Re-scaled getThrottle() and getSteering() for increased sensitivity.

Added a SpeedRacerDriveNew for closed loop testing.
Increased Climb Winch power to full power.
Added soft limits for climber.
Added ShooterAdjustHood() command.
Removed some old methods from MayhemTalonSRX.
Various minor naming fixes.

Author: coach-streeter

src/main/java/frc/robot/Robot.java

@@ -35,12 +35,10 @@ public class Robot extends TimedRobot {
   @Override
   @SuppressWarnings("deprecation")
   public void robotInit() {
-    // Instantiate our RobotContainer. This will perform all our button bindings,
-    // and put our
-    // autonomous chooser on the dashboard.
+    // Instantiate our RobotContainer.  This will perform all our button bindings,
+    // and put our autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
 
-    // TODO:  Improve below to display something like "Code Last Loaded:  7 minutes ago"
     SmartDashboard.putString("Robot Loaded on:", new Date().toLocaleString());
   }
 

src/main/java/org/mayheminc/robot2020/RobotContainer.java

@@ -75,6 +75,15 @@ private void configureDefaultCommands() {
         drive.setDefaultCommand(new DriveDefault());
         // intake.setDefaultCommand(new IntakeExtenderVBus());
         magazine.setDefaultCommand(new MagazineDefault());
+
+        // TODO:  Figure out if the current approach of "AirCompressorDefault()" is the way to go for compressor control.
+        // KBS doesn't think the below is the right way to have the compressor be on "by default" because
+        // it would require there to always be a command running to keep the compressor off.  However, that
+        // is a good way to ensure it doesn't get left off by accident.  Not quite sure how to handle this;
+        // would really rather that other commands which need the compressor off (such as a high-power command
+        // which wants all the battery power available) would turn the compressor off when the command starts
+        // and off when the command ends.)  Then again, maybe the "defaultCommand" is a good way to do this
+        // and I just don't understand the style yet.
         compressor.setDefaultCommand(new AirCompressorDefault());
     }
 
@@ -188,22 +197,24 @@ private void configureOperatorPadButtons() {
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollers(-1.0));
 
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_SEVEN.whileHeld(new TurretAimToTarget());
-
+        OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(1.0));
+        
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_NINE.whenPressed(new ClimberSetPistons(true));
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_TEN.whenPressed(new ClimberSetPistons(false));
 
-        OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(1.0));
+
         // OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whenPressed(new
         // IntakeSetPosition(RobotContainer.intake.PIVOT_UP));
         // OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whenPressed(new
         // IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
         OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whileHeld(new ShooterSetTurretVBus(-0.2));
         OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whileHeld(new ShooterSetTurretVBus(+0.2));
-        OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whileHeld(new ShooterSetHoodVBus(+1.0));
-        OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whileHeld(new ShooterSetHoodVBus(-1.0));
+        OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whileHeld(new ShooterAdjustHood(+1000.0));
+        OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whileHeld(new ShooterAdjustHood(-1000.0));
 
-        OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whileHeld(new ClimberSetWinchesPower(0.7));
-        OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whileHeld(new ClimberSetWinchesPower(-0.7));
+        // TODO:  Consider if below should use "variable power" for climber or just always full speed?
+        OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whileHeld(new ClimberSetWinchesPower(1.0));
+        OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whileHeld(new ClimberSetWinchesPower(-1.0));
 
         // OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_UP.whenPressed(new
         // MagazineSetTurntable());

src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java

@@ -35,7 +35,7 @@ public DriveStraightOnHeading(double arg_targetSpeed, DistanceUnits units, doubl
 		addRequirements(RobotContainer.drive);
 
 		if (units == DistanceUnits.INCHES) {
-			arg_distance = arg_distance / Drive.DISTANCE_PER_PULSE;
+			arg_distance = arg_distance / Drive.DISTANCE_PER_PULSE_IN_INCHES;
 		}
 		m_targetPower = arg_targetSpeed;
 		m_desiredDisplacement = Math.abs(arg_distance);

src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustHood.java

@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package org.mayheminc.robot2020.commands;
+
+import org.mayheminc.robot2020.RobotContainer;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class ShooterAdjustHood extends CommandBase {
+
+  double m_adjust;
+
+  /**
+   * Creates a new ShooterAdjustHood.
+   */
+  public ShooterAdjustHood(double adjust) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.shooter);
+
+    m_adjust = adjust;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    RobotContainer.shooter.setHoodPosition(RobotContainer.shooter.getHoodPosition() + m_adjust);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java

@@ -17,6 +17,8 @@ public class AirCompressor extends SubsystemBase {
    * Creates a new compressor.
    */
   public AirCompressor() {
+    // turn on the compressor in the constructor
+    setCompressor(true);
   }
 
   public void setCompressor(boolean b) {

src/main/java/org/mayheminc/robot2020/subsystems/Climber.java

@@ -18,6 +18,9 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Climber extends SubsystemBase {
+  private static final int MAX_HEIGHT_SOFT_LIMIT = 640000;
+  private static final int MIN_HEIGHT_SOFT_LIMIT = 5000;
+
   private final MayhemTalonSRX winchLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_LEFT);
   private final MayhemTalonSRX winchRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_RIGHT);
   // private final MayhemTalonSRX walkerLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_LEFT);
@@ -37,11 +40,19 @@ public Climber() {
     winchLeft.configNominalOutputVoltage(+0.0f, -0.0f);
     winchLeft.configPeakOutputVoltage(+12.0, -12.0);
     winchLeft.setInverted(true);
+    winchLeft.configForwardSoftLimitThreshold(MAX_HEIGHT_SOFT_LIMIT);
+    winchLeft.configForwardSoftLimitEnable(true);
+    winchLeft.configReverseSoftLimitThreshold(MIN_HEIGHT_SOFT_LIMIT);
+    winchLeft.configReverseSoftLimitEnable(true);
 
     winchRight.setNeutralMode(NeutralMode.Brake);
     winchRight.configNominalOutputVoltage(+0.0f, -0.0f);
     winchRight.configPeakOutputVoltage(+12.0, -12.0);
     winchRight.setInverted(false);
+    winchRight.configForwardSoftLimitThreshold(MAX_HEIGHT_SOFT_LIMIT);
+    winchRight.configForwardSoftLimitEnable(true);
+    winchRight.configReverseSoftLimitThreshold(MIN_HEIGHT_SOFT_LIMIT);
+    winchRight.configReverseSoftLimitEnable(true);
 
     // walkerRight.setNeutralMode(NeutralMode.Brake);
     // walkerRight.configNominalOutputVoltage(+0.0f, -0.0f);
@@ -50,7 +61,6 @@ public Climber() {
     // walkerLeft.setNeutralMode(NeutralMode.Brake);
     // walkerLeft.configNominalOutputVoltage(+0.0f, -0.0f);
     // walkerLeft.configPeakOutputVoltage(+12.0, -12.0);
-
   }
 
   public void zero() {

src/main/java/org/mayheminc/robot2020/subsystems/Drive.java

@@ -55,11 +55,11 @@ public class Drive extends SubsystemBase {
 
 	// Drive parameters
 	// pi * diameter * (pulley ratios) / (counts per rev * gearbox reduction)
-	public static final double DISTANCE_PER_PULSE = 3.14 * 5.75 * 36.0 / 42.0 / (2048.0 * 7.56); // corrected for 2020
+	public static final double DISTANCE_PER_PULSE_IN_INCHES = 3.14 * 5.75 * 36.0 / 42.0 / (2048.0 * 7.56); // corrected for 2020
 
 	private boolean m_closedLoopMode = false;
-	private double m_maxWheelSpeed = 1.0; // set to 1.0 as default for "open loop" percentVBus drive
-	private double m_voltageRampRate = 48.0;
+	private double m_maxWheelSpeed = 1.0;    // should be maximum wheel speed in native units
+	private static final double CLOSED_LOOP_RAMP_RATE = 0.1;    // time from neutral to full in seconds
 
 	private double m_initialWheelDistance = 0.0;
 	private int m_iterationsSinceRotationCommanded = 0;
@@ -102,39 +102,30 @@ public Drive() {
 		m_HeadingPid.setAbsoluteTolerance(kToleranceDegreesPIDControl);
 
 		// confirm all four drive talons are in coast mode
-		leftFrontTalon.setNeutralMode(NeutralMode.Coast);
-		leftRearTalon.setNeutralMode(NeutralMode.Coast);
-		rightFrontTalon.setNeutralMode(NeutralMode.Coast);
-		rightRearTalon.setNeutralMode(NeutralMode.Coast);
+		leftFrontTalon.setNeutralMode(NeutralMode.Brake);
+		leftRearTalon.setNeutralMode(NeutralMode.Brake);
+		rightFrontTalon.setNeutralMode(NeutralMode.Brake);
+		rightRearTalon.setNeutralMode(NeutralMode.Brake);
 
 		// set rear talons to follow their respective front talons
-		leftRearTalon.changeControlMode(ControlMode.Follower);
-		leftRearTalon.set(leftFrontTalon.getDeviceID());
-
-		rightRearTalon.changeControlMode(ControlMode.Follower);
-		rightRearTalon.set(rightFrontTalon.getDeviceID());
+		leftRearTalon.follow(leftFrontTalon);
+		rightRearTalon.follow(rightFrontTalon);
 
 		// the left motors move the robot forwards with positive power
 		// but the right motors are backwards.
 		leftFrontTalon.setInverted(false);
 		leftRearTalon.setInverted(false);
 		rightFrontTalon.setInverted(true);
 		rightRearTalon.setInverted(true);
-
-		// sensor phase is reversed, since there are 3 reduction stages in the gearbox
-		leftFrontTalon.setSensorPhase(true);
-		leftRearTalon.setSensorPhase(true);
-		rightFrontTalon.setSensorPhase(true);
-		rightRearTalon.setSensorPhase(true);
 	}
 
 	public void init() {
 		// reset the NavX
 		zeroHeadingGyro(0.0);
 
 		// talon closed loop config
-		configureCanTalon(leftFrontTalon);
-		configureCanTalon(rightFrontTalon);
+		configureDriveTalon(leftFrontTalon);
+		configureDriveTalon(rightFrontTalon);
 	}
 
 	public void zeroHeadingGyro(double headingOffset) {
@@ -155,29 +146,22 @@ public void initDefaultCommand() {
 		// setDefaultCommand(new SpeedRacerDrive());
 	}
 
-	private void configureCanTalon(MayhemTalonSRX talon) {
+	private void configureDriveTalon(MayhemTalonSRX talon) {
 		double wheelP = 1.5;
 		double wheelI = 0.0;
 		double wheelD = 0.0;
 		double wheelF = 1.0;
 
 		talon.setFeedbackDevice(FeedbackDevice.IntegratedSensor);
-
-		// talon.reverseSensor(false);
+	
 		talon.configNominalOutputVoltage(+0.0f, -0.0f);
 		talon.configPeakOutputVoltage(+12.0, -12.0);
 
-		if (m_closedLoopMode) {
-			talon.changeControlMode(ControlMode.Velocity);
-			m_maxWheelSpeed = 270;
-		} else {
-			talon.changeControlMode(ControlMode.PercentOutput);
-			m_maxWheelSpeed = 1.0;
-		}
-
-		talon.setPID(wheelP, wheelI, wheelD, wheelF, 0, m_voltageRampRate, 0);
-
-		// talon.enableControl();
+		talon.config_kP(0, wheelP);
+		talon.config_kI(0, wheelI);
+		talon.config_kD(0, wheelD);
+		talon.config_kF(0, wheelF);
+		talon.configClosedloopRamp(CLOSED_LOOP_RAMP_RATE);  // specify minimum time for neutral to full in seconds
 
 		DriverStation.reportError("setWheelPIDF: " + wheelP + " " + wheelI + " " + wheelD + " " + wheelF + "\n", false);
 	}
@@ -210,16 +194,10 @@ public void toggleClosedLoopMode() {
 
 	public void setClosedLoopMode() {
 		m_closedLoopMode = true;
-		// reconfigure the "master" drive talons now that we're in closed loop mode
-		configureCanTalon(leftFrontTalon);
-		configureCanTalon(rightFrontTalon);
 	}
 
 	public void setOpenLoopMode() {
 		m_closedLoopMode = false;
-		// reconfigure the "master" drive talons now that we're in open loop mode
-		configureCanTalon(leftFrontTalon);
-		configureCanTalon(rightFrontTalon);
 	}
 
 	// ********************* ENCODER-GETTERS ************************************
@@ -417,6 +395,89 @@ public void setAutoAlignFalse() {
 	}
 
 	public void speedRacerDrive(double throttle, double rawSteeringX, boolean quickTurn) {
+		speedRacerDriveNew(throttle, rawSteeringX, quickTurn);
+	}
+
+
+	public void speedRacerDriveNew(double throttle, double rawSteeringX, boolean quickTurn) {
+		double leftPower, rightPower;
+		double rotation = 0;
+		final double QUICK_TURN_GAIN = 0.55; // 2019: .75. 2020: .75 was too fast.
+
+		int throttleSign;
+		if (throttle >= 0.0) {
+			throttleSign = 1;
+		} else {
+			throttleSign = -1;
+		}
+
+		// check for if steering input is essentially zero for "DriveStraight" functionality
+		if ((-0.01 < rawSteeringX) && (rawSteeringX < 0.01)) {
+			// no turn being commanded, drive straight or stay still
+			m_iterationsSinceRotationCommanded++;
+			if ((-0.01 < throttle) && (throttle < 0.01)) {
+				// no motion commanded, stay still
+				m_iterationsSinceMovementCommanded++;
+				rotation = 0.0;
+				m_desiredHeading = getHeading(); // whenever motionless, set desired heading to current heading
+				// reset the PID controller loop now that we have a new desired heading
+				resetAndEnableHeadingPID();
+			} else {
+				// driving straight
+				if ((m_iterationsSinceRotationCommanded == LOOPS_GYRO_DELAY)
+						|| (m_iterationsSinceMovementCommanded >= LOOPS_GYRO_DELAY)) {
+					// exactly LOOPS_GYRO_DELAY iterations with no commanded turn,
+					// or haven't had movement commanded for longer than LOOPS_GYRO_DELAY,
+					// so we want to take steps to preserve our current heading hereafter
+
+					// get current heading as desired heading
+					m_desiredHeading = getHeading();
+
+					// reset the PID controller loop now that we have a new desired heading
+					resetAndEnableHeadingPID();
+					rotation = 0.0;
+				} else if (m_iterationsSinceRotationCommanded < LOOPS_GYRO_DELAY) {
+					// not long enough since we were last turning,
+					// just drive straight without special heading maintenance
+					rotation = 0.0;
+				} else if (m_iterationsSinceRotationCommanded > LOOPS_GYRO_DELAY) {
+					// after more then LOOPS_GYRO_DELAY iterations since commanded turn,
+					// maintain the target heading
+					rotation = maintainHeading();
+				}
+				m_iterationsSinceMovementCommanded = 0;
+			}
+		} else {
+			// commanding a turn, reset iterationsSinceRotationCommanded
+			m_iterationsSinceRotationCommanded = 0;
+			m_iterationsSinceMovementCommanded = 0;
+		}
+
+		if (quickTurn) {
+			// want a high-rate turn (also allows "spin" behavior)
+			// power to each wheel is a combination of the throttle and rotation
+			rotation = rawSteeringX * throttleSign * QUICK_TURN_GAIN;
+			leftPower = throttle + rotation;
+			rightPower = throttle - rotation;
+		} else {
+			// want a standard rate turn (scaled by the throttle)
+			if (rawSteeringX >= 0.0) {
+				// turning to the right, derate the right power by turn amount
+				// note that rawSteeringX is positive in this portion of the "if"
+				leftPower = throttle;
+				rightPower = throttle * (1.0 - Math.abs(rawSteeringX));
+			} else {
+				// turning to the left, derate the left power by turn amount
+				// note that rawSteeringX is negative in this portion of the "if"
+				leftPower = throttle * (1.0 - Math.abs(rawSteeringX));
+				rightPower = throttle;
+			}
+		}
+		setMotorPower(leftPower, rightPower);
+	}
+
+
+	public void speedRacerDriveOld(double throttle, double rawSteeringX, boolean quickTurn) {
 		double leftPower, rightPower;
 		double rotation = 0;
 		double adjustedSteeringX = rawSteeringX * throttle;
@@ -498,7 +559,6 @@ public void speedRacerDrive(double throttle, double rawSteeringX, boolean quickT
 		leftPower = throttle + rotation;
 		rightPower = throttle - rotation;
 		setMotorPower(leftPower, rightPower);
-
 	}
 
 	public int stage = 0;
@@ -597,9 +657,9 @@ public void updateSmartDashboard() {
 		// b - divide by 12 (1 foot per 12 inches)
 		// c - multiply by distance (in inches) per pulse
 		SmartDashboard.putNumber("Left Speed (fps)",
-				leftFrontTalon.getSelectedSensorVelocity(0) * 10 / 12 * DISTANCE_PER_PULSE);
+				leftFrontTalon.getSelectedSensorVelocity(0) * 10 / 12 * DISTANCE_PER_PULSE_IN_INCHES);
 		SmartDashboard.putNumber("Right Speed (fps)",
-				rightFrontTalon.getSelectedSensorVelocity(0) * 10 / 12 * DISTANCE_PER_PULSE);
+				rightFrontTalon.getSelectedSensorVelocity(0) * 10 / 12 * DISTANCE_PER_PULSE_IN_INCHES);
 
 		SmartDashboard.putNumber("Left Talon Output Voltage", leftFrontTalon.getMotorOutputVoltage());
 		SmartDashboard.putNumber("Right Talon Output Voltage", rightFrontTalon.getMotorOutputVoltage());