public Tensioner() {
super(
"Tensioner",
RobotPreferences.getTensionerP(),
RobotPreferences.getTensionerI(),
RobotPreferences.getTensionerD());
getPIDController().setContinuous(false);
getPIDController()
.setInputRange(
RobotPreferences.getTensionerMinVolts(), RobotPreferences.getTensionerMaxVolts());
getPIDController().setOutputRange(TENSIONER_MIN_MOTOR_SPEED, TENSIONER_MAX_MOTOR_SPEED);
getPIDController().setAbsoluteTolerance(threshold_volts);
setSetpoint(RobotPreferences.getTensionerDefaultVolts());
// TODO: Enable this once the PID Controllers are
// tuned.
// enable();
}
/**
* The Tensioner class manages the robot mechanism which controls the tension of a kicking system
* comprised of bands which store energy in order to "kick" the game ball. The tensioner has a close
* relationship and must work in concert with the Leg subystem.
*
* <p>The Tensioner has three logical states:
*
* <p>- High Tension (ready to kick) - Low Tension (ready to "putt") - Slack Tension
*
* <p>A tensioning bar is controlled by a motor, and limit switches are used to define the limits of
* motion.
*
* <p>An Potentiometer sensor measures the position of the tension bar, from which the current
* tension level is derived.
*
* <p>In order to maintain high tension, a locking mechanism is provided, controlled by a relay.
* This lock must be enabled in order to maintain a high tension level.
*/
public class Tensioner extends PIDSubsystem {
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
SpeedController leftSC =
RobotMap.tensionerLeftSC; // Positive == Detension; Negative == Tensioning
DigitalInput leftMin = RobotMap.tensionerLeftMin; // true == Low Tension
DigitalInput leftMax = RobotMap.tensionerLeftMax; // true == High Tension
AnalogChannel sensor = RobotMap.tensionerSensor;
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
static double TENSIONER_MIN_MOTOR_SPEED = -1;
static double TENSIONER_MAX_MOTOR_SPEED = 1;
public static final int kTensionSlack = 0;
public static final int kTensionMin = 1;
public static final int kTensionMax = 2;
public static final int kTensionIntermediate = 3;
double min_tension_volts = RobotPreferences.getTensionerMinVolts();
double slack_tension_volts = RobotPreferences.getTensionerSlackVolts();
double max_tension_volts = RobotPreferences.getTensionerMaxVolts();
double threshold_volts = RobotPreferences.getTensionerOnTargetToleranceVolts();
public Tensioner() {
super(
"Tensioner",
RobotPreferences.getTensionerP(),
RobotPreferences.getTensionerI(),
RobotPreferences.getTensionerD());
getPIDController().setContinuous(false);
getPIDController()
.setInputRange(
RobotPreferences.getTensionerMinVolts(), RobotPreferences.getTensionerMaxVolts());
getPIDController().setOutputRange(TENSIONER_MIN_MOTOR_SPEED, TENSIONER_MAX_MOTOR_SPEED);
getPIDController().setAbsoluteTolerance(threshold_volts);
setSetpoint(RobotPreferences.getTensionerDefaultVolts());
// TODO: Enable this once the PID Controllers are
// tuned.
// enable();
}
public boolean isDetensioned() {
double curr_volts = sensor.getAverageVoltage();
boolean on_target =
((curr_volts > (slack_tension_volts - threshold_volts))
&& (curr_volts < (slack_tension_volts + threshold_volts)));
boolean at_min = leftMin.get();
return (on_target || at_min);
}
public double getTensionLevel() {
return sensor.getAverageVoltage();
}
public boolean isEnabled() {
return this.getPIDController().isEnable();
}
// Put methods for controlling this subsystem
// here. Call these from Commands.
public void initDefaultCommand() {
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
// Set the default command for a subsystem here.
// setDefaultCommand(new MySpecialCommand());
}
// Returns current tension level in volts
protected double returnPIDInput() {
return sensor.getAverageVoltage();
}
// Controls output of both left and right motors
// NOTE: Only one motor may actually be used;
// in this case, the right speed controller will
// not be used.
protected void usePIDOutput(double output) {
//
// Important: invert the motor polarity!
// (Positive Motor Voltage -> Lower Tension)
//
output = -output;
//
// Only allow the arms to move in a direction away from a limit switch
// if that limit switch is closed, unless the motor command is 0.0
// (so the motor controller can be disabled)
//
if ((output > 0.0 && !leftMin.get()) || (output < 0.0 && !leftMax.get()) || (output == 0.0))
this.leftSC.set(output);
else this.leftSC.set(0);
}
}