/**
* Constructor
*
* @param topJaguarChannel The PWM channel for the Jaguar for the top motor
* @param bottomJaguarChannel The PWM channel for the Jaguar for the bottom motor;
* @param topEncoderA The Digital I/O channel for channel A of the encoder for the top motor
* @param topEncoderB The Digital I/O channel for channel B of the encoder for the top motor
* @param bottomEncoderA The Digital I/O channel for channel A of the encoder for the bottom motor
* @param bottomEncoderB The Digital I/O channel for channel B of the encoder for the bottom motor
* @param limitSwitchChannel The Digital I/O channel for the gripper limit switch
*/
public Gripper(
int topJaguarChannel,
int bottomJaguarChannel,
int topEncoderA,
int topEncoderB,
int bottomEncoderA,
int bottomEncoderB,
int limitSwitchChannel) {
topMotor = new Jaguar(topJaguarChannel);
bottomMotor = new Jaguar(bottomJaguarChannel);
topEncoder = new Encoder(topEncoderA, topEncoderB);
topEncoder.start();
topEncoder.reset();
bottomEncoder = new Encoder(bottomEncoderA, bottomEncoderB);
bottomEncoder.start();
bottomEncoder.reset();
topTarget = 0;
bottomTarget = 0;
limitSwitch = new DigitalInput(limitSwitchChannel);
ds = DriverStation.getInstance();
}
/**
* 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 static void init() {
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
driveLeftSpeedController = new Talon(9);
LiveWindow.addActuator("Drive", "Left Speed Controller", (Talon) driveLeftSpeedController);
driveRightSpeedController = new Talon(8);
LiveWindow.addActuator("Drive", "Right Speed Controller", (Talon) driveRightSpeedController);
driveRobotDrive21 = new RobotDrive(driveLeftSpeedController, driveRightSpeedController);
driveRobotDrive21.setSafetyEnabled(true);
driveRobotDrive21.setExpiration(0.1);
driveRobotDrive21.setSensitivity(0.5);
driveRobotDrive21.setMaxOutput(1.0);
driveLeftEncoder = new Encoder(2, 3, false, EncodingType.k4X);
LiveWindow.addSensor("Drive", "LeftEncoder", driveLeftEncoder);
driveLeftEncoder.setDistancePerPulse(1.0);
driveLeftEncoder.setPIDSourceType(PIDSourceType.kRate);
driveRightEncoder = new Encoder(0, 1, false, EncodingType.k4X);
LiveWindow.addSensor("Drive", "RightEncoder", driveRightEncoder);
driveRightEncoder.setDistancePerPulse(1.0);
driveRightEncoder.setPIDSourceType(PIDSourceType.kRate);
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
}
/** The log method puts interesting information to the SmartDashboard. */
public void log() {
SmartDashboard.putNumber("Left Distance", left_encoder.getDistance());
SmartDashboard.putNumber("Right Distance", right_encoder.getDistance());
SmartDashboard.putNumber("Left Speed", left_encoder.getRate());
SmartDashboard.putNumber("Right Speed", right_encoder.getRate());
SmartDashboard.putNumber("Gyro", gyro.getAngle());
}
public void enable() {
rightEncoder.reset();
leftEncoder.reset();
rightEncoder.start();
leftEncoder.start();
// pidRight.enable();
// pidLeft.enable();
} // end enable
static {
// stealerEncoder.setDistancePerPulse(.0245);
stealerEncoder.setMaxPeriod(1 /* seconds */);
stealerEncoder.reset();
lassoCounter.reset();
controller.setOutputRange(-0.2, 0.2);
controller.setAbsoluteTolerance(50);
}
// basic arcadeDrive: y=forward/backward speed, x=left/right speed
public void arcadeDrive(double y, double x) {
SmartDashboard.putNumber("RightEncoder", rightEncoder.get());
SmartDashboard.putNumber("LeftEncoder", leftEncoder.get());
Preferences p = Preferences.getInstance();
final boolean kReverseDirection = p.getBoolean("DriveTrainReverseDirection", false);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, kReverseDirection);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, kReverseDirection);
robotDrive.arcadeDrive(y, x);
} // end arcadeDrive
// 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 disable() {
setSetpoint(0.0);
// pidRight.disable();
// pidLeft.disable();
leftMotor.set(0.0);
rightMotor.set(0.0);
rightEncoder.reset();
leftEncoder.reset();
} // end disable
public double getRPM() {
if (Timer.getFPGATimestamp() > RPMUpdatePeriod + encoderMarkTime) {
double encoderCPS =
(enc.get() - encoderMarkCounts) / (Timer.getFPGATimestamp() - encoderMarkTime);
encoderRPM = (encoderCPS * 60) / 16;
encoderMarkCounts = enc.get();
encoderMarkTime = Timer.getFPGATimestamp();
}
return encoderRPM;
}
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
public void RotateFrame(double s, double distance, double dir) {
reset();
left.set(dir * s);
right.set(dir * s);
do {
SmartDashboard.putNumber("left distance", lEncoder.getDistance());
SmartDashboard.putNumber("right distance", rEncoder.getDistance());
} while (rEncoder.getDistance() <= distance);
left.set(0);
right.set(0);
}
public void DriveForward(double s, double distance) {
reset();
left.set(s);
right.set(-1 * s);
do {
SmartDashboard.putNumber("left distance", lEncoder.getDistance());
SmartDashboard.putNumber("right distance", rEncoder.getDistance());
} while (rEncoder.getDistance() <= distance);
left.set(0);
right.set(0);
reset();
}
// 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
}
private double m_shoot() {
if (encoder.get() >= desired || shootTime.get() > m_timeToShoot) {
shooting = false;
retracting = true;
shootTime.stop();
retractTime.start();
retractTime.reset();
return .1;
} else { // encoder < desired && shootTime < 0.2
error = desired - encoder.get();
return -1;
}
}
// lowers elevator partway for last pull-up at end
public void lowerElevatorPartway() {
if (climberEncoder.get() > lowerElevatorPartwayLimit) {
if (!getError(false) || climberEncoder.get() > errorUpperLimit) {
climberMotor.set(-elevatorSpeed);
} else {
if (!getUpperLimit()) {
climberMotor.set(elevatorSpeed);
}
}
} else {
climberMotor.set(0);
}
putData();
}
private Drivetrain() {
pdp = new PowerDistributionPanel();
left = new Talon(kLeftDriveMotorPWM);
right = new Talon(kRightDriveMotorPWM);
lEncoder = new Encoder(kLeftDriveEncoderA, kLeftDriveEncoderB, false, EncodingType.k1X);
lEncoder.setDistancePerPulse(kDistancePerPulse);
lEncoder.reset();
rEncoder = new Encoder(kRightDriveEncoderA, kRightDriveEncoderB, false, EncodingType.k1X);
rEncoder.setDistancePerPulse(kDistancePerPulse);
rEncoder.reset();
lP = new PID("left", left, rEncoder);
rP = new PID("Right", right, rEncoder);
lP.setAbsoluteTolerance(1.0);
rP.setAbsoluteTolerance(1.0);
}
public static void init() {
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
driveTrainLeftFront = new Talon(0);
LiveWindow.addActuator("DriveTrain", "Left Front", (Talon) driveTrainLeftFront);
driveTrainLeftRear = new Talon(1);
LiveWindow.addActuator("DriveTrain", "Left Rear", (Talon) driveTrainLeftRear);
driveTrainRightFront = new Talon(2);
LiveWindow.addActuator("DriveTrain", "Right Front", (Talon) driveTrainRightFront);
driveTrainRightRear = new Talon(3);
LiveWindow.addActuator("DriveTrain", "Right Rear", (Talon) driveTrainRightRear);
driveTrainHolonomicDrive =
new RobotDrive(
driveTrainLeftFront, driveTrainLeftRear, driveTrainRightFront, driveTrainRightRear);
driveTrainHolonomicDrive.setSafetyEnabled(false);
driveTrainHolonomicDrive.setExpiration(0.1);
driveTrainHolonomicDrive.setSensitivity(0.5);
driveTrainHolonomicDrive.setMaxOutput(1.0);
driveTrainHolonomicDrive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
driveTrainHolonomicDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
driveTrainGyro = new AnalogGyro(0);
LiveWindow.addSensor("DriveTrain", "Gyro", driveTrainGyro);
driveTrainGyro.setSensitivity(0.007);
forkliftMotor = new Talon(4);
LiveWindow.addActuator("Forklift", "Motor", (Talon) forkliftMotor);
forkliftEncoder = new Encoder(0, 1, false, EncodingType.k4X);
LiveWindow.addSensor("Forklift", "Encoder", forkliftEncoder);
forkliftEncoder.setDistancePerPulse(0.012);
forkliftEncoder.setPIDSourceType(PIDSourceType.kDisplacement);
toteCaptureLeftWheel = new Talon(8);
LiveWindow.addActuator("Tote Capture", "Left Wheel", (Talon) toteCaptureLeftWheel);
toteCaptureRightWheel = new Talon(9);
LiveWindow.addActuator("Tote Capture", "Right Wheel", (Talon) toteCaptureRightWheel);
containerCaptureMotor = new Talon(10);
LiveWindow.addActuator("Container Capture", "Motor", (Talon) containerCaptureMotor);
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
powerDistributionPanel = new PowerDistributionPanel();
}
public void arcadeDrive() {
lT = OI.getDriveStick().getLeftX() - OI.getDriveStick().getLeftY();
rT = OI.getDriveStick().getLeftX() + OI.getDriveStick().getLeftY();
// maxV = Math.max(Math.abs(lT), Math.abs(rT));
if (maxV > 1) {
lT = lT / maxV;
rT = rT / maxV;
}
left.set(lT);
right.set(rT);
SmartDashboard.putNumber("left distance while driving", lEncoder.getDistance());
SmartDashboard.putNumber("right distance while driving", rEncoder.getDistance());
}
public Shooter(int encoderA, int encoderB, int victor1, int victor2, int victor3) {
encoder = new Encoder(encoderA, encoderB);
encoder.start();
encoder.reset();
motor1 = new Victor(victor1); // 2
motor2 = new Victor(victor2); // 5
motor3 = new Victor(victor3); // 4
time = new Timer();
shootTime = new Timer();
retractTime = new Timer();
time.start();
time.reset();
}
public void robotInit() {
j1 = new Joystick(1);
j2 = new Joystick(0);
shooter = new Victor(2);
shooter2 = new Victor(4);
intake = new Victor(3);
right = new Victor(0);
left = new Victor(1);
rd = new RobotDrive(left, right);
light = new DigitalInput(3);
count = 0;
comp = new Compressor();
inup = new DoubleSolenoid(0, 1);
inup2 = new DoubleSolenoid(2, 3);
gyro = new AnalogGyro(0);
gyro.setSensitivity(.007);
gyro.calibrate();
gyro.reset();
enc = new Encoder(1, 2, true, Encoder.EncodingType.k4X);
enc.setDistancePerPulse(.11977); // circumference of wheel/200 (PPR)
enc.setPIDSourceType(PIDSourceType.kDisplacement);
enc.reset();
out =
new PIDOutput() {
@Override
public void pidWrite(double out) {
rd.setLeftRightMotorOutputs(out, out);
}
};
controller = new PIDController(.007, 0, 0, enc, out);
controller.setAbsoluteTolerance(.25);
controller.disable();
gcontroller = new PIDController(kp, ki, kd, gyro, out);
gcontroller.setAbsoluteTolerance(ktol);
gcontroller.disable();
timerStart = -1;
timeToCancel = -1;
}
// 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
private double m_retract() {
if (encoder.get() <= 0 || retractTime.get() >= 1.0) {
shooting = false;
retracting = false;
return 0;
} else { // encoder > 0 && retractTime < 1.0
return .1;
}
}
// moves elevator to starting position and sets encoder
public void homingSequence() {
if (homeUp) {
while (!getUpperLimit()) {
climberMotor.set(homingSpeed);
}
while (getUpperLimit()) {
climberMotor.set(-homingReverseSpeed);
}
climberEncoder.reset();
} else {
while (!getLowerLimit()) {
climberMotor.set(-homingSpeed);
}
while (getLowerLimit()) {
climberMotor.set(homingReverseSpeed);
}
climberEncoder.reset();
}
}
private OI() {
us = new AnalogChannel(US_PORT_1);
us2 = new AnalogChannel(US_PORT_2);
us3 = new AnalogChannel(US_PORT_3);
quad = new Encoder(QUAD_PORT_1, QUAD_PORT_2);
quadL = new Encoder(QUAD_PORT_3, QUAD_PORT_4);
quadR = new Encoder(QUAD_PORT_5, QUAD_PORT_6);
quad.start();
quadL.start();
quadR.start();
pot = new AnalogChannel(POT_PORT);
stick = new Joystick(JOYSTICK_PORT);
stick2 = new Joystick(JOYSTICK_PORT_2);
j2b1 = new JoystickButton(stick2, 1);
j2b2 = new JoystickButton(stick2, 2);
j2b3 = new JoystickButton(stick2, 3);
j2b4 = new JoystickButton(stick2, 4);
}
public boolean driveStraight(double dist) {
SmartDashboard.putNumber("RightEncoder", rightEncoder.get());
SmartDashboard.putNumber("LeftEncoder", leftEncoder.get());
Preferences p = Preferences.getInstance();
L_encoderVal =
leftEncoder.get()
/ p.getDouble(
"LeftEncoderRatio",
0.0); // converts encoder value to inches; encoder ratio should be about 58.76
R_encoderVal =
rightEncoder.get()
/ p.getDouble("RightEncoderRatio", 0.0); // converts encoder value to inches
encoderVal = (R_encoderVal); // only one encoder
encoderErr = dist - encoderVal;
distOut =
encoderErr * p.getDouble("Encoder_kP", 0.0)
+ (Math.abs(encoderErr) / encoderErr)
* p.getDouble(
"OutputMin",
0.0); // Encoder kP = 0.5; abs(x)/x returns sign of x; 0.2 is the min. magnitude
gyroErr = gyro.getAngle(); // Setpoint is always 0
double angleOut = gyroErr * p.getDouble("Gyro_kP", 0.0) + .3; // * distOut / Math.abs(distOut);
SmartDashboard.putNumber("encoderErr", encoderErr);
SmartDashboard.putNumber("distOut", distOut);
SmartDashboard.putNumber("gyroErr", gyro.getAngle());
SmartDashboard.putNumber("angleOut", angleOut);
if (Math.abs(encoderErr) < p.getDouble("DistBuffer", 0.0)
&& Math.abs(gyroErr) < p.getDouble("AngleBuffer", 0.0)) {
// leftEncoder.reset();
// rightEncoder.reset();
// gyro.reset();
// arcadeDrive(0, 0);
return true; // returns true when robot gets to its goal
} else {
arcadeDrive(
-distOut,
angleOut); // Gyro kP = 1/18.0; Arcade Drive uses reversed rotate values (neg. goes Left /
// pos. goes Right)
return false; // returns false if robot still hasn't reached its goal yet
}
} // end driveStraight
public void tankDrive(double leftSpeed, double rightSpeed) {
Preferences p = Preferences.getInstance();
final boolean kReverseDirection = p.getBoolean("DriveTrainReverseDirection", false);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, kReverseDirection);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, kReverseDirection);
// if (kReverseDirection) {
// robotDrive.tankDrive(rightSpeed, leftSpeed);
// return;
// }
robotDrive.tankDrive(leftSpeed, rightSpeed);
SmartDashboard.putNumber("gyroErr", gyro.getAngle());
SmartDashboard.putNumber("encoder", rightEncoder.get());
} // end tankDrive
/**
* Constructor for leadscrew class.
*
* @param dsModule digital sidecar module number
* @param motor1Chan first motor PWM channel
* @param motor2Chan second motor PWM channel
* @param encAChan encoder 'A' channel
* @param encBChan encoder 'B' channel
* @param homeSwitchChan homing switch input channel
*/
public Leadscrew(
int dsModule,
int motor1Chan,
int motor2Chan,
int encAChan,
int encBChan,
int homeSwitchChan) {
motor1 = new Jaguar(dsModule, motor1Chan);
motor2 = new Jaguar(dsModule, motor2Chan);
// controller = new BangBangController();
final int switchDebounceCycles = 3;
// homeSwitch = new Button(dsModule, homeSwitchChan, switchDebounceCycles);
encoder = new Encoder(dsModule, encAChan, dsModule, encBChan);
encoder.setDistancePerPulse(RobotConfiguration.leadscrewRatio);
encoder.setReverseDirection(false);
controller = new BangBangController(motor1, tolerance, false);
Reset();
}
// raises the elevator, tries again if hooks don't catch
public void raiseElevator() {
if (!getUpperLimit()) {
if (!getError(true) || climberEncoder.get() < errorLowerLimit) {
climberMotor.set(elevatorSpeed);
} else {
if (!getLowerLimit()) {
climberMotor.set(-elevatorSpeed);
}
}
} else {
climberMotor.set(0);
}
putData();
}
public DriveTrain() {
super();
front_left_motor = new Talon(1);
back_left_motor = new Talon(2);
front_right_motor = new Talon(3);
back_right_motor = new Talon(4);
drive = new RobotDrive(front_left_motor, back_left_motor, front_right_motor, back_right_motor);
left_encoder = new Encoder(1, 2);
right_encoder = new Encoder(3, 4);
// Encoders may measure differently in the real world and in
// simulation. In this example the robot moves 0.042 barleycorns
// per tick in the real world, but the simulated encoders
// simulate 360 tick encoders. This if statement allows for the
// real robot to handle this difference in devices.
if (Robot.isReal()) {
left_encoder.setDistancePerPulse(0.042);
right_encoder.setDistancePerPulse(0.042);
} else {
// Circumference in ft = 4in/12(in/ft)*PI
left_encoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
right_encoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
}
rangefinder = new AnalogInput(6);
gyro = new AnalogGyro(1);
// Let's show everything on the LiveWindow
LiveWindow.addActuator("Drive Train", "Front_Left Motor", (Talon) front_left_motor);
LiveWindow.addActuator("Drive Train", "Back Left Motor", (Talon) back_left_motor);
LiveWindow.addActuator("Drive Train", "Front Right Motor", (Talon) front_right_motor);
LiveWindow.addActuator("Drive Train", "Back Right Motor", (Talon) back_right_motor);
LiveWindow.addSensor("Drive Train", "Left Encoder", left_encoder);
LiveWindow.addSensor("Drive Train", "Right Encoder", right_encoder);
LiveWindow.addSensor("Drive Train", "Rangefinder", rangefinder);
LiveWindow.addSensor("Drive Train", "Gyro", gyro);
}