Changes from first night after GSD event: (1) added current limits to drive motor code, (2) tuned heading correction code in drive to drive straight without wandering, and (3) added some "DriveStraightOnHeading()" commands to the driver pad "start" and "back" buttons for testing.

Author: coach-streeter

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

@@ -19,6 +19,7 @@
 
 import org.mayheminc.robot2020.autonomousroutines.*;
 import org.mayheminc.robot2020.commands.*;
+import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
 import org.mayheminc.robot2020.subsystems.*;
 
 /**
@@ -196,14 +197,15 @@ private void configureDriverPadButtons() {
         DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterWheelSetVBus(0.0));
         DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED));
 
-        DRIVER_PAD.DRIVER_PAD_BACK_BUTTON.whileHeld(new DriveStraight(0.1));
-
         DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenHeld(new ShooterFiringSequence(60.0));
         DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenReleased(new ShooterCeaseFire());
 
         DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new ShooterCloseFiringSequence(60.0));
         DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whenReleased(new ShooterCeaseFire());
 
+        DRIVER_PAD.DRIVER_PAD_BACK_BUTTON.whenPressed(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 240, 0));
+        DRIVER_PAD.DRIVER_PAD_START_BUTTON.whenPressed(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 240, 0));
+
         // DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new
         // FeederSet(1.0));
 

src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java

@@ -26,7 +26,7 @@ public StartBWTrench(double extraDistance) {
                 addCommands(new StartBWShoot3());
 
                 // then, drive to the trench entrance (jog left a little to get there)
-                addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 46, 140));
+                addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 30, 140));
 
                 // pick up balls while heading down the trench.
                 addCommands(new ParallelCommandGroup(
@@ -35,7 +35,7 @@ public StartBWTrench(double extraDistance) {
                                 // ensure the hood is down
                                 new HoodSetAbsWhileHeld(Hood.STARTING_POSITION),
                                 // drive the path under the control panel to the end
-                                new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 140 + extraDistance, 180)));
+                                new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 148 + extraDistance, 180)));
 
                 // now driven to the balls at far end of trench
                 addCommands(new Wait(0.8), // wait for last two balls to get into robot
@@ -44,7 +44,7 @@ public StartBWTrench(double extraDistance) {
 
                 // after getting all three balls, go back to shooting position
                 // first, make sure we drive straight out from under the control panel
-                addCommands(new DriveStraightOnHeading(-0.6, DistanceUnits.INCHES, 8 + extraDistance, 180));
+                addCommands(new DriveStraightOnHeading(-0.6, DistanceUnits.INCHES, 16 + extraDistance, 180));
                 addCommands(new IntakeSetRollers(0.0)); // turn off the intake in case it has been stalled for a while
 
                 // drive diagonally over towards the shooting position, while turning on shooter

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

@@ -51,8 +51,9 @@ public class Drive extends SubsystemBase implements PidTunerObject {
 	// NavX parameters
 	private double m_desiredHeading = 0.0;
 	private boolean m_useHeadingCorrection = true;
-	private static final double HEADING_PID_P = 0.007; // was 0.030 in 2019 for HIGH_GEAR; was 0.007 in early 2020
-	private static final double HEADING_PID_D = 0.080; // was 0.04 in 2019
+	private static final double HEADING_PID_P = 0.010; // was 0.007 at GSD; was 0.030 in 2019 for HIGH_GEAR
+	private static final double HEADING_PID_I = 0.000; // was 0.000 at GSD; was 0.000 in 2019
+	private static final double HEADING_PID_D = 0.080; // was 0.080 at GSD; was 0.04 in 2019
 	private static final double kToleranceDegreesPIDControl = 0.2;
 
 	// Drive parameters
@@ -97,8 +98,8 @@ public Drive() {
 		m_HeadingError = new PIDHeadingError();
 		m_HeadingError.m_Error = 0.0;
 		m_HeadingCorrection = new PIDHeadingCorrection();
-		m_HeadingPid = new PIDController(HEADING_PID_P, 0.000, HEADING_PID_D, m_HeadingError, m_HeadingCorrection,
-				0.020 /* period in seconds */);
+		m_HeadingPid = new PIDController(HEADING_PID_P, HEADING_PID_I, HEADING_PID_D, m_HeadingError,
+				m_HeadingCorrection, 0.020 /* period in seconds */);
 		m_HeadingPid.setInputRange(-180.0f, 180.0f);
 		m_HeadingPid.setContinuous(true); // treats the input range as "continous" with wrap-around
 		m_HeadingPid.setOutputRange(-.50, .50); // set the maximum power to correct twist
@@ -110,6 +111,26 @@ public Drive() {
 		rightFrontTalon.setNeutralMode(NeutralMode.Coast);
 		rightRearTalon.setNeutralMode(NeutralMode.Coast);
 
+		// configure output voltages
+		leftFrontTalon.configNominalOutputVoltage(+0.0f, -0.0f);
+		leftRearTalon.configNominalOutputVoltage(+0.0f, -0.0f);
+		rightFrontTalon.configNominalOutputVoltage(+0.0f, -0.0f);
+		rightRearTalon.configNominalOutputVoltage(+0.0f, -0.0f);
+		leftFrontTalon.configPeakOutputVoltage(+12.0, -12.0);
+		leftRearTalon.configPeakOutputVoltage(+12.0, -12.0);
+		rightFrontTalon.configPeakOutputVoltage(+12.0, -12.0);
+		rightRearTalon.configPeakOutputVoltage(+12.0, -12.0);
+
+		// configure current limits
+		// enabled = true
+		// 40 = limit (amps)
+		// 60 = trigger_threshold (amps)
+		// 0.5 = threshold time(s)
+		leftFrontTalon.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 40, 60, 0.5));
+		leftRearTalon.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 40, 60, 0.5));
+		rightFrontTalon.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 40, 60, 0.5));
+		rightRearTalon.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 40, 60, 0.5));
+
 		// set rear talons to follow their respective front talons
 		leftRearTalon.follow(leftFrontTalon);
 		rightRearTalon.follow(rightFrontTalon);
@@ -247,13 +268,6 @@ public void setHeadingCorrectionMode(final boolean useHeadingCorrection) {
 	}
 
 	private void resetAndEnableHeadingPID() {
-		// if (Robot.shifter.getGear() == Shifter.HIGH_GEAR) {
-		m_HeadingPid.setP(HEADING_PID_P);
-		// } else
-		// {
-		// low gear
-		// m_HeadingPid.setP(HEADING_PID_P_FOR_LOW_GEAR);
-		// }
 		m_HeadingPid.reset();
 		m_HeadingPid.enable();
 	}
@@ -606,6 +620,8 @@ private double maintainHeading() {
 
 		// check for major heading changes and take action to prevent
 		// integral windup if there is a major heading error
+		// TODO: In 2020, this code was causing "wandering" with non-zero HEADING_PID_I.
+		// Worked around issue by setting HEADING_PID_I = 0
 		if (Math.abs(m_HeadingError.m_Error) > 10.0) {
 			if (!m_HeadingPidPreventWindup) {
 				m_HeadingPid.setI(0.0);
@@ -614,7 +630,7 @@ private double maintainHeading() {
 			}
 		} else {
 			m_HeadingPidPreventWindup = false;
-			m_HeadingPid.setI(0.001);
+			m_HeadingPid.setI(HEADING_PID_I);
 		}
 
 		return headingCorrection;
@@ -676,6 +692,11 @@ public void updateSmartDashboard() {
 		SmartDashboard.putNumber("Left Talon Output Voltage", leftFrontTalon.getMotorOutputVoltage());
 		SmartDashboard.putNumber("Right Talon Output Voltage", rightFrontTalon.getMotorOutputVoltage());
 
+		SmartDashboard.putNumber("LF Falcon Supply Current", leftFrontTalon.getSupplyCurrent());
+		SmartDashboard.putNumber("LR Falcon Supply Current", leftRearTalon.getSupplyCurrent());
+		SmartDashboard.putNumber("RF Falcon Supply Current", rightFrontTalon.getSupplyCurrent());
+		SmartDashboard.putNumber("RR Falcon Supply Current", rightRearTalon.getSupplyCurrent());
+
 		SmartDashboard.putBoolean("Closed Loop Mode", m_closedLoopMode);
 		SmartDashboard.putBoolean("Speed Racer Drive Mode", m_speedRacerDriveMode);