Constructing the robot program
All the motors are inverted based on the gearing and mounting of the motors. Adding gears reverses the direction of the motor for each gear added.
The elevator is controlled by a potentiometer and driven with a motor connected with a Victor speed controller. Since this is a PIDSubsystem, it has P, I, and D constants and a Tolerance value that determines when the elevator has reached it's target. The target is used by commands to know when they are finished for sequential operations.
The wrist is a PIDSubsystem with a Victor speed controller motor and a potentiometer to measure the wrist angle. The P, I, and D values are used with the built-in PID controller and the tolerance is used to determine if the wrist has reached the desired angle.
Subsystems and commands
There are commands that open and close the claw. It has no sensor and is operated for a fixed time rather than from sensor feedback. It is generally better to have some sensor feedback to control the subsystem rather than relying on time, but this is simply a Vex motor and running it for a longer time than necessary won't hurt anything. For larger FRC motors, the motor could be easily damaged if it runs open-loop like this.
This command has the robot driving forward for a fixed period of time, in this case about 0.5 seconds. The command starts the motors running, then uses a timeout to stop them after driving for a while. The end() method stops the robot from driving and the interrupted() method simply calls the end() method.
DriveWithJoystick calls the method on the drive base to drive the robot using the joystick that is created as part of the operator interface. This command never finishes because it's the default command for the drive base subsystem so the robot can drive using the joystick when it's not doing anything else.
Drive base code for mecanum drive
the DriveBase subsysem has a default command (DriveWithJoystick) that allows the user to drive the robot when no other command is using the drive base. It drives using the mecanum drive methods as part of the RobotDrive class. The speed is inverted for forward driving because the joystick has positive (forward) values when the stick is pulled backwards.