/** Handles periodic operations. Should be called periodically. */
public void loop() {
ds.setDigitalOut(8, limitSwitch.get());
final int tolerance =
10; // the number of degrees, in either direction, of tolerance for motor movement
if (!isAccepting) {
if (topTarget > topEncoder.get() && Math.abs(topTarget - topEncoder.get()) > tolerance) {
int error = Math.abs(topTarget - topEncoder.get());
topMotor.set(-limit(error / ERROR_CONVERSION));
// SmartDashboard.log("Backwards", "Gripper Top Motor");
} else if (topTarget < topEncoder.get()
&& Math.abs(topTarget - topEncoder.get()) > tolerance) {
double error = Math.abs(topTarget - topEncoder.get());
topMotor.set(limit(error / ERROR_CONVERSION));
// SmartDashboard.log("Forward", "Gripper Top Motor");
} else {
topMotor.set(0);
}
if (bottomTarget > bottomEncoder.get()
&& Math.abs(bottomTarget - bottomEncoder.get()) > tolerance) {
double error = Math.abs(bottomTarget - bottomEncoder.get());
bottomMotor.set(-limit(error / ERROR_CONVERSION));
// SmartDashboard.log("Backwards", "Gripper Bottom Motor");
} else if (bottomTarget < bottomEncoder.get()
&& Math.abs(bottomTarget - bottomEncoder.get()) > tolerance) {
double error = Math.abs(bottomTarget - bottomEncoder.get());
bottomMotor.set(limit(error / ERROR_CONVERSION));
// SmartDashboard.log("Forward", "Gripper Bottom Motor");
} else {
bottomMotor.set(0);
// SmartDashboard.log("Stopped", "Gripper Bottom Motor");
}
}
// if the gripper is in accept mode, it needs to stop when the limit switch is pressed
if (isAccepting && !limitSwitch.get()) {
// and also by simply stopping them
topMotor.set(0);
bottomMotor.set(0);
isAccepting = false;
// stop the motors by making the target positions equal to the current positions
topTarget = topEncoder.get();
bottomTarget = bottomEncoder.get();
} else if (isAccepting && limitSwitch.get()) {
// if it is accepting and the limit switch is not pressed
topMotor.set(1);
bottomMotor.set(-1);
}
}
public double doRobotdriveScaling(double value) {
if (driverstation.getDigitalIn(Constants.driverstation_scale_toggle_channel)) {
if (driverstation.getDigitalIn(Constants.driverstation_scale_toggle_channel2)) {
driverstation.setDigitalOut(2, true);
driverstation.setDigitalOut(1, false);
driverstation.setDigitalOut(3, false);
if (driverstation.getAnalogIn(Constants.driverstation_scale_channel) < 1) {
return value * driverstation.getAnalogIn(Constants.driverstation_scale_channel);
}
driverstation.setDigitalOut(3, true);
return 0;
}
driverstation.setDigitalOut(3, false);
driverstation.setDigitalOut(1, true);
driverstation.setDigitalOut(2, false);
return value * driverstation.getAnalogIn(Constants.driverstation_scale_channel) * 0.2;
}
driverstation.setDigitalOut(3, false);
driverstation.setDigitalOut(1, false);
driverstation.setDigitalOut(2, false);
return value * Constants.robotdrive_scale_hi;
}
