/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public void robotInit() {
black = new Jaguar(4, 1);
red = new Jaguar(4, 2);
leftEncoder = new Encoder(4, 5);
rightEncoder = new Encoder(6, 7);
left = new Joystick(1);
right = new Joystick(2);
gamePad = new Joystick(3);
watchdog = Watchdog.getInstance();
dsLCD = DriverStationLCD.getInstance();
photoreceptorL = new DigitalInput(4, 1);
photoreceptorM = new DigitalInput(4, 2);
photoreceptorR = new DigitalInput(4, 3);
camera = AxisCamera.getInstance();
driveMode = 0; // 0 = Tank; 1 = Arcade; 2 = Kaj
driveToggle = false;
cruiseControl = false;
camera.writeResolution(AxisCamera.ResolutionT.k160x120);
camera.writeWhiteBalance(AxisCamera.WhiteBalanceT.hold);
camera.writeExposureControl(AxisCamera.ExposureT.hold);
camera.writeExposurePriority(AxisCamera.ExposurePriorityT.frameRate);
leftEncoder.start();
rightEncoder.start();
}
public void enable() {
rightEncoder.reset();
leftEncoder.reset();
rightEncoder.start();
leftEncoder.start();
// pidRight.enable();
// pidLeft.enable();
} // end enable
// starts encoders
public DriveTrain() {
leftEncoder.start();
rightEncoder.start();
gyro.reset();
Preferences p = Preferences.getInstance();
if (!p.containsKey("DriveTrainShootLimitVoltage")) {
p.putDouble("DriveTrainShootLimitVoltage", kDefaultShootLimitVoltage);
}
if (!p.containsKey("DriveTrainQuickTurnP")) {
p.putDouble("DriveTrainQuickTurnP", kDefaultQuickTurnProportion);
}
if (!p.containsKey("DriveTrainQuickTurnDeadband")) {
p.putDouble("DriveTrainQuickTurnDeadband", kDefaultQuickTurnDeadband);
}
if (!p.containsKey("DriveTrainReverseDirection")) {
p.putBoolean("DriveTrainReverseDirection", kDefaultReverseDirection);
}
if (!p.containsKey("LeftEncoderRatio")) {
p.putDouble("LeftEncoderRatio", 4.875);
}
if (!p.containsKey("RightEncoderRatio")) {
p.putDouble("RightEncoderRatio", 4.875);
}
if (!p.containsKey("Encoder_kP")) {
p.putDouble("Encoder_kP", 0.05);
}
if (!p.containsKey("OutputMin")) {
p.putDouble("OutputMin", .2);
}
if (!p.containsKey("DistBuffer")) {
p.putDouble("DistBuffer", 1);
}
if (!p.containsKey("Gyro_kP")) {
p.putDouble("Gyro_kP", 1 / 18);
}
if (!p.containsKey("AngleBuffer")) {
p.putDouble("AngleBuffer", 4.5);
}
if (!p.containsKey("AutoDist_0")) {
p.putDouble("AutoDist_0", 0.0);
}
if (!p.containsKey("AutoDist_1")) {
p.putDouble("AutoDist_1", 0.0);
}
if (!p.containsKey("AutoDist_2")) {
p.putDouble("AutoDist_2", 0.0);
}
if (!p.containsKey("AutoDist_3")) {
p.putDouble("AutoDist_3", 0.0);
}
if (!p.containsKey("ShiftWhenTurningThreshold")) {
p.putDouble("ShiftWhenTurningThreshold", 0.5);
}
if (!p.containsKey("ShiftWhenTurningDelay")) {
p.putDouble("ShiftWhenTurningDelay", 1.5);
}
} // end constructor
public void enable() {
rightEncoder.reset();
leftEncoder.reset();
rightEncoder.start();
leftEncoder.start();
gyro.reset();
final boolean kReverseDirection =
Preferences.getInstance().getBoolean("DriveTrainReverseDirection", false);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, kReverseDirection);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, kReverseDirection);
} // end enable
// starts encoders
public DriveTrain() {
leftEncoder.start();
rightEncoder.start();
gyro.reset();
Preferences p = Preferences.getInstance();
if (!p.containsKey("DriveTrainShootLimitVoltage")) {
p.putDouble("DriveTrainShootLimitVoltage", kDefaultShootLimitVoltage);
}
if (!p.containsKey("DriveTrainQuickTurnP")) {
p.putDouble("DriveTrainQuickTurnP", kDefaultQuickTurnProportion);
}
if (!p.containsKey("DriveTrainQuickTurnDeadband")) {
p.putDouble("DriveTrainQuickTurnDeadband", kDefaultQuickTurnDeadband);
}
if (!p.containsKey("DriveTrainReverseDirection")) {
p.putBoolean("DriveTrainReverseDirection", kDefaultReverseDirection);
}
} // end constructor
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
public static void init() {
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
driveTrainLeftFront = new Talon(1, 1);
LiveWindow.addActuator("Drive Train", "Left Front", (Talon) driveTrainLeftFront);
driveTrainRightFront = new Talon(1, 2);
LiveWindow.addActuator("Drive Train", "Right Front", (Talon) driveTrainRightFront);
driveTrainLeftRear = new Talon(1, 3);
LiveWindow.addActuator("Drive Train", "Left Rear", (Talon) driveTrainLeftRear);
driveTrainRightRear = new Talon(1, 4);
LiveWindow.addActuator("Drive Train", "Right Rear", (Talon) driveTrainRightRear);
driveTrainRobotDrive =
new RobotDrive(
driveTrainLeftFront, driveTrainLeftRear, driveTrainRightFront, driveTrainRightRear);
driveTrainRobotDrive.setSafetyEnabled(true);
driveTrainRobotDrive.setExpiration(0.1);
driveTrainRobotDrive.setSensitivity(0.5);
driveTrainRobotDrive.setMaxOutput(1.0);
driveTrainRobotDrive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
driveTrainRobotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
driveTrainRobotDrive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
driveTrainRobotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
armWheelsShaftMotor = new Talon(1, 5);
LiveWindow.addActuator("Arm Wheels", "Shaft Motor", (Talon) armWheelsShaftMotor);
platformMotor = new Talon(1, 6);
LiveWindow.addActuator("Platform", "Motor", (Talon) platformMotor);
platformShaftEncoder = new Encoder(1, 1, 1, 2, false, EncodingType.k4X);
LiveWindow.addSensor("Platform", "Shaft Encoder", platformShaftEncoder);
platformShaftEncoder.setDistancePerPulse(1.0);
platformShaftEncoder.setPIDSourceParameter(PIDSourceParameter.kRate);
platformShaftEncoder.start();
platformLimitSwitchFront = new DigitalInput(1, 3);
LiveWindow.addSensor("Platform", "Limit Switch Front", platformLimitSwitchFront);
platformLimitSwitchBack = new DigitalInput(1, 4);
LiveWindow.addSensor("Platform", "Limit Switch Back", platformLimitSwitchBack);
platformTestFrontTestLimitSwitch = new DigitalInput(1, 5);
LiveWindow.addSensor(
"Platform Test", "Front Test Limit Switch", platformTestFrontTestLimitSwitch);
platformTestTestMotor = new Talon(1, 7);
LiveWindow.addActuator("Platform Test", "Test Motor", (Talon) platformTestTestMotor);
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
}
// Have to start the encoder to begin reading.
public void initQuadEncoder() {
quadEncoderSensor.start();
}
public void start() {
enc.start();
}
