/**
* ******************************
*
* <p>Configures motors: - Sets which side is inverted - Configures the secondary motors to be in
* follow mode - Sets ramp rate
*
* <p>******************************
*/
private void setupMotors() {
leftMotor1.setInverted(invertLeft);
leftMotor2.setInverted(invertLeft);
rightMotor1.setInverted(invertRight);
rightMotor1.setInverted(invertRight);
leftMotor2.changeControlMode(CANTalon.TalonControlMode.Follower);
leftMotor2.set(leftMotor1.getDeviceID());
rightMotor2.changeControlMode(CANTalon.TalonControlMode.Follower);
rightMotor2.set(rightMotor1.getDeviceID());
leftMotor1.setVoltageRampRate(RAMP_RATE);
rightMotor1.setVoltageRampRate(RAMP_RATE);
}
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public void robotInit() {
chooser = new SendableChooser();
chooser.addDefault("Default Auto", defaultAuto);
chooser.addObject("My Auto", customAuto);
SmartDashboard.putData("Auto choices", chooser);
controlMethod = new SendableChooser();
controlMethod.addDefault("Tank Control", tankControl);
controlMethod.addObject("Stick Control", stickControl);
controlMethod.addObject("Clayton Control", claytonControl);
SmartDashboard.putData("Control method", controlMethod);
rightMotor = new CANTalon(RIGHT_INDEX);
rightSlave = new CANTalon(RIGHT_INDEX + 1);
rightSlave.changeControlMode(CANTalon.TalonControlMode.Follower);
rightSlave.set(RIGHT_INDEX);
rightMotor.setInverted(true);
leftMotor = new CANTalon(LEFT_INDEX);
leftSlave = new CANTalon(LEFT_INDEX + 1);
leftSlave.changeControlMode(CANTalon.TalonControlMode.Follower);
leftSlave.set(LEFT_INDEX);
intakeMotor = new CANTalon(INTAKE_INDEX);
// Arm motor is currently disabled
// armMotor = new CANTalon(ARM_INDEX);
compressor = new Compressor(0);
compressor.setClosedLoopControl(true);
intakePneumatic = new DoubleSolenoid(1, 2);
intakePneumatic.set(DoubleSolenoid.Value.kOff);
gamepad = new Gamepad(0);
}
/**
* This method inverts the motor direction.
*
* @param inverted specifies true to invert motor direction, false otherwise.
*/
@Override
public void setInverted(boolean inverted) {
super.setInverted(inverted);
} // setInverted
/** Initializes all actuators */
public static void init() {
// TODO: Change ID's
// Motors
rightDriveMotor = new CANTalon(3);
rightDriveMotor =
initCANTalon(
rightDriveMotor,
FeedbackDevice.QuadEncoder,
RIGHT_DIVE_REVERSE_SENSOR,
CODES_PER_REV,
DRIVE_MOTOR_ACCEPTABLE_ERROR,
RIGHT_DRIVE_KP,
RIGHT_DRIVE_KI,
RIGHT_DRIVE_KD);
rightDriveMotor2 = new CANTalon(0);
rightDriveMotor2.changeControlMode(CANTalon.TalonControlMode.Follower);
rightDriveMotor2.setInverted(true);
rightDriveMotor2.set(rightDriveMotor.getDeviceID());
leftDriveMotor = new CANTalon(2);
leftDriveMotor =
initCANTalon(
leftDriveMotor,
FeedbackDevice.QuadEncoder,
LEFT_DIVE_REVERSE_SENSOR,
CODES_PER_REV,
DRIVE_MOTOR_ACCEPTABLE_ERROR,
LEFT_DRIVE_KP,
LEFT_DRIVE_KI,
LEFT_DRIVE_KD);
leftDriveMotor.setInverted(true);
leftDriveMotor2 = new CANTalon(1);
leftDriveMotor2.changeControlMode(CANTalon.TalonControlMode.Follower);
leftDriveMotor2.setInverted(true);
leftDriveMotor2.set(leftDriveMotor.getDeviceID());
armWinchMotor1 = new VictorSP(4);
armWinchMotor2 = new VictorSP(5);
armWinchMotor2.setInverted(true);
armAngleMotor = new CANTalon(6);
armAngleMotor =
initCANTalon(
armAngleMotor,
FeedbackDevice.AnalogPot,
ARM_REVERSE_SENSOR,
ARM_POT_TURNS_PER_REV,
ARM_ACCEPTABLE_EROR,
ARM_ANGLE_KP,
ARM_ANGLE_KI,
ARM_ANGLE_KD);
// TODO: Use string pot with CANTalon
boulderIntakeMotor = new VictorSP(7);
catapultMotor = new VictorSP(8);
// Solenoids
driveShiftPneumatic = new Solenoid(2);
winchRatchetPneumatic = new Solenoid(1);
}