/**
* ************************************
*
* <p>Does the configuration required for basic drive. So, when switching to basic drive this gets
* called and now the robot will work with basic drive.
*
* <p>************************************
*/
public void setBasic() {
current = profiles.BASIC;
leftMotor1.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
rightMotor1.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
}
/**
* 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);
}
/**
* ************************************
*
* <p>Does the configuration required for speed drive. So, when switching to speed drive this gets
* called and now the robot will work with speed drive.
*
* <p>************************************
*/
public void setSpeedMode() {
current = profiles.SPEED;
leftMotor1.setProfile(0);
rightMotor1.setProfile(0);
leftMotor1.changeControlMode(CANTalon.TalonControlMode.Speed);
rightMotor1.changeControlMode(CANTalon.TalonControlMode.Speed);
}
/**
* ************************************
*
* <p>Does the configuration required for positional drive. So, when switching to positional drive
* this gets called and now the robot will work with positional drive.
*
* <p>************************************
*/
public void setPositional() {
current = profiles.POSITIONAL;
leftMotor1.setProfile(0);
rightMotor1.setProfile(0);
// rightMotor1.reverseSensor(true);
leftMotor1.changeControlMode(CANTalon.TalonControlMode.Position);
rightMotor1.changeControlMode(CANTalon.TalonControlMode.Position);
}
/**
* ******************************
*
* <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);
}
// static initializer
public static void initialize() {
if (!initialized) {
gamepad = new Joystick(GAMEPAD_ID);
initialized = true;
teleopMode = false;
// create and initialize arm motor
frontArmMotor = new CANTalon(FRONT_ARM_MOTOR_ID);
if (frontArmMotor != null) {
System.out.println("Initializing front arm motor (position control)...");
// VERY IMPORTANT - resets talon faults to render them usable again!!
frontArmMotor.clearStickyFaults();
frontArmMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder);
frontArmMotor.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
// set brake mode
frontArmMotor.enableBrakeMode(true);
// initializes encoder to zero
frontArmMotor.setPosition(0);
} else System.out.println("ERROR: Front Arm motor not initialized!");
// create and initialize roller motor
frontArmRollerMotor = new CANTalon(FRONT_ARM_ROLLER_ID);
if (frontArmRollerMotor != null) {
System.out.println("Initializing front arm roller motor (PercentVbus control)...");
frontArmRollerMotor.clearStickyFaults();
// set up roller motor for percent Vbus control mode
frontArmRollerMotor.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
frontArmRollerMotor.enableBrakeMode(false);
// initializes speed of rollers to zero
frontArmRollerMotor.set(0);
} else System.out.println("ERROR: Front Arm roller motor not initialized!");
}
}
public Drivetrain() {
frontLeft = new CANTalon(RobotMap.FRONT_LEFT_MOTOR);
frontLeft.changeControlMode(CANTalon.TalonControlMode.Follower);
frontLeft.enableControl();
rearLeft = new CANTalon(RobotMap.REAR_LEFT_MOTOR);
rearLeft.enableControl();
frontRight = new CANTalon(RobotMap.FRONT_RIGHT_MOTOR);
frontRight.changeControlMode(CANTalon.TalonControlMode.Follower);
frontRight.enableControl();
rearRight = new CANTalon(RobotMap.REAR_RIGHT_MOTOR);
rearRight.enableControl();
drive = new RobotDrive(rearLeft, rearRight);
drive.setInvertedMotor(MotorType.kRearLeft, true);
drive.setInvertedMotor(MotorType.kRearRight, true);
}
public MasterCANTalon(int port) {
talon = new CANTalon(port);
talon.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Relative);
talon.reverseSensor(false);
talon.setProfile(0);
talon.setF(0.1722);
talon.setP(0.1);
talon.setI(0);
talon.setD(0);
talon.configNominalOutputVoltage(+0.0f, -0.0f);
talon.configPeakOutputVoltage(+12.0f, -12.0f);
talon.changeControlMode(TalonControlMode.Speed);
}
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 TalonSRX setSpeed(double speed) {
talon.changeControlMode(TalonControlMode.PercentVbus);
talon.set(speed);
return this;
}
@Override
public double getSpeed() {
talon.changeControlMode(TalonControlMode.PercentVbus);
return talon.get();
}
/** 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);
}