Drive Subsystem

The Drive Subsystem performs the same function as its counterpart in the Robot Java tutorial, namely it will control the two drive motors. In fact you may want to refer to your code for that previous robot as an aid.

The motors for this new robot will use the same PWMMotor class that you used previously. There is a slight difference, however. These motors are connected to an Auxiliary Arduino board and are addressed slightly differently. You will need to use the following constructor:

robotCore.PWMMotor.PWMMotor(
    int 	pwmPin,
    int 	dirPin,
    int 	i2cAddr 
  )

You are, of course, going to need the pin numbers and i2c address to use:

  private final static int k_i2cAddr = 4;
  private final static int k_leftPWMPin = 5;
  private final static int k_leftDirPin = 4;
  private final static int k_rightPWMPin = 6;
  private final static int k_rightDirPin = 7;

With this information, you should now be able to create your Drive Subsystem class. Once you have done that, create an Arcade Drive command and get your robot driving with a joystick. Remember to set your Arcade Drive command as the Drive Subsystem’s default command.

Once you have your robot driving you will want to use the wheel encoders to calibrate the speed as we did in the Robot Java tutorial. For this robot however, we are using a different kind of encoder called a Quadrature Encoder. We will still use the Encoder class, but we will set the type to EncoderType.Quadrature. Once again, these encoders are connected to the Auxiliary Arduino board, so you will need to use this constructor:

robotCore.Encoder.Encoder(
    EncoderType 	type,
    int 	pin1,
    int 	pin2,
    int 	i2cAddr 
  )

You will need the pin numbers (the i2c address is the same as you used for the motors):

  private final static int k_leftEncoderPin1 = 2;
  private final static int k_leftEncoderPin2 = 8;
  private final static int k_rightEncoderPin1 = 3;
  private final static int k_rightEncoderPin2 = 9;

Once you have created instances of your encoders, remember to set them as the feedback devices for your motors.

Finally, create setPower and setSpeed functions to control the two motors. You can look back at the Robot Java tutorial if you need help in remembering how to do this.

You should now be ready to tune the speed of your drive motors.  Again, you can look back at the Robot Java tutorial to refresh your memory of how to do this but the following are the basic steps involved:

1. You should create a Calibrate Drive command and tie it to a button.
2. Using this command, drive the motors at full power (using setPower) and log the speed for the left and right wheels. This will tell you the maximum possible speed for each of the motors.
3. First set the F term of the PID controller for each of the motors so that they are running at about 50% of the maximum speed (using setSpeed).
4. Next add a P term and increase it until you see the speeds become unstable, and then back off a bit.
5. Finally add an I term and increase it until you see the speeds become unstable, and then back off a bit. Remember to set an appropriate IZone.
6. Once you have it tuned for 50% speed, check the tuning at 25% and 75%. Only change the P and I terms if there are significant problems at those speeds.

In my case, I found that the max speed was around 500 and the final calibration graphs appeared as follows:

Once you have your drive motors calibrated, you can change your Arcade Drive command to use speed instead of power.

Next Shooter Subsystem