public static void changeControlMode(ControlMode mode) {
// Set control mode for the CAN Talon motor controllers
mecanumDriveControlsLeftFront.changeControlMode(mode);
mecanumDriveControlsLeftRear.changeControlMode(mode);
mecanumDriveControlsRightFront.changeControlMode(mode);
mecanumDriveControlsRightRear.changeControlMode(mode);
// Makes sure the Feedback Device is a Quad Encoder
mecanumDriveControlsLeftFront.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder);
mecanumDriveControlsLeftRear.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder);
mecanumDriveControlsRightFront.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder);
mecanumDriveControlsRightRear.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder);
// Sets PID Values for the Mecanum Drive Train
if (mode.equals(ControlMode.Speed)) {
mecanumDriveControlsLeftFront.setPID(1,0.002,1.0,0.0001,255,200,0);
mecanumDriveControlsLeftRear.setPID(1,0);
mecanumDriveControlsRightFront.setPID(1,0);
mecanumDriveControlsRightRear.setPID(1,0);
}
}
/**
* Turns the robot.
* @param left Whether to turn left (true),or right (false)
*/
public void turn(boolean left){
for (int i = 0; i < motors.size(); i++) {
RobotMap.changeControlMode(ControlMode.Speed);
if (left) {
robotDrive.mecanumDrive_Cartesian(0,-.5,0);
}
else {
robotDrive.mecanumDrive_Cartesian(0,.5,0);
}
}
}