/**
* ******************************
*
* <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);
}
public DriveTrain() {
// super supplies the PID constants (.05, 0, 0)
super("DriveTrain", .0070, .0003, 0);
robotDrive = new RobotDrive(_leftMaster, _rightMaster);
// Invert the appropriate controllers
// robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
_leftSlave1.changeControlMode(TalonControlMode.Follower);
_leftSlave1.set(_leftMaster.getDeviceID());
_leftSlave2.changeControlMode(TalonControlMode.Follower);
_leftSlave2.set(_leftMaster.getDeviceID());
_rightSlave1.changeControlMode(TalonControlMode.Follower);
_rightSlave1.set(_rightMaster.getDeviceID());
_rightSlave2.changeControlMode(TalonControlMode.Follower);
_rightSlave2.set(_rightMaster.getDeviceID());
ahrs = Robot.ahrs;
// getPIDController() returns the PID Controller that is included with
// classes that extend PIDSubsystems.
turnController = getPIDController();
turnController.disable();
turnController.setInputRange(-180, 180);
turnController.setOutputRange(-.7, .7);
// begin vision test
// frame = RobotMap.FRAME;
// session = RobotMap.CAMERA_SESSION;
// NIVision.IMAQdxConfigureGrab(session);
// colorTable = new NIVision.RawData();
// end vision test
}
@Override
public int getDeviceID() {
return talon.getDeviceID();
}
/** 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);
}