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 void setIndexMotor(int direction) {
wheelDirection = direction;
if (direction == 1) {
indexMotor.set(1);
} else if (direction == -1) {
indexMotor.set(-1);
} else {
indexMotor.disable();
}
}
// lowers the elevator, tries again if hooks don't catch
public void lowerElevator() {
if (!getLowerLimit()) {
if (!getError(false) || climberEncoder.get() > errorUpperLimit) {
climberMotor.set(-elevatorSpeed);
} else {
if (!getUpperLimit()) {
climberMotor.set(elevatorSpeed);
}
}
} else {
climberMotor.set(0);
}
putData();
}
// 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();
}
// sets climber speed to given value
public void manualControl(double d) {
if (topClimberSwitch.get() && d > 0 || bottomClimberSwitch.get() && d < 0) {
climberMotor.set(0);
}
if (topClimberSwitch.get() && d < 0) {
climberMotor.set(d);
}
if (bottomClimberSwitch.get() && d > 0) {
climberMotor.set(d);
}
if (!topClimberSwitch.get() && !bottomClimberSwitch.get()) {
climberMotor.set(d);
}
putData();
}
protected void usePIDOutput(double output) {
// Use output to drive your system, like a motor
// e.g. yourMotor.set(output);
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=OUTPUT
talon1.pidWrite(output);
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=OUTPUT
}
// 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();
}
}
public boolean quickTurn(double setpoint) {
// Proportional quickturn algorithm
Preferences p = Preferences.getInstance();
final double kP = p.getDouble("DriveTrainQuickTurnP", 0.0);
final double kDB = p.getDouble("DriveTrainQuickTurnDeadband", 0.0);
double angle = -getGyro(); // -driveTrain.getGyro() b/c the gyro is upsidedown
double error = setpoint - angle;
double twist = error * kP;
double magnitude = Math.abs(twist);
if (magnitude < kDB) {
twist = kDB * (twist > 0.0 ? 1.0 : -1.0);
} else if (magnitude > 1.0) {
twist = (twist > 0.0 ? 1.0 : -1.0);
}
SmartDashboard.putNumber("DriveTrainGyro", angle);
leftMotor.set(-twist);
rightMotor.set(twist);
// Done once we pass the setpoint
return Math.abs(angle) >= Math.abs(setpoint);
} // end quickTurn
public void tankDrive(double leftSpeed, double rightSpeed) {
leftMotor.set(leftSpeed);
rightMotor.set(rightSpeed);
SmartDashboard.putNumber("DriveTrainGyro", -gyro.getAngle()); // upside down
} // end tankDrive
public void pidWrite(double output) {
leftMotor.set(output);
}
public void pidWrite(double output) {
rightMotor.set(output);
}
public void setFeederIntakeSpeed(double value) {
feederIntake.set(-value);
}
