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); } }