Re-order drive velocity PID tuning routine

README.md

@@ -24,6 +24,10 @@ For maximal flexibility, Road Runner has no dependencies on Android or the [FTC
 
 The process is pretty much the same for all drives. Simply extend `TankDrive`, `MecanumDrive`, or `SwerveDrive`, create a constructor that initializes the appropriate `DcMotor` instances, and use them to implement `getWheelPositions()`/`setMotorPowers()`. Often, it's also a good idea to put follower-related logic into the drive classes (this is the approach taken in the quickstart). If you decide to use one of the prebuilt drive classes (e.g., `SampleMecanumDriveOptimzed`), **make sure you update `DriveConstants` to reflect your robot**.
 
+### Drive Velocity PID Tuning (optional)
+
+If you plan on using the built-in velocity PID (i.e., `RUN_USING_ENCODER`), it's important to tune the coefficients for your robot (it is recommended you use the built-in velocity PID). Run `DriveVelocityPIDTuner` and adjust the PID gains to minimize the error as best you can. Finally, uncomment the `setPIDCoefficients()` stub at the bottom of your drive constructor and fill in the new coefficients.
+
 ### Drive Characterization
 
 To determine the proper open loop powers, it's necessary to determine the relationship between robot velocity/acceleration and motor voltage. That is, we want to find kV, kStatic, and kA in the following relation: power = kV * v + kA * a + kStatic (there are some subtleties here; for more details see [this paper](https://www.chiefdelphi.com/media/papers/3402)). In theory, kV = 1 / max velocity and kA = 1 / max acceleration.
@@ -44,7 +48,7 @@ Now your tuned drive class can be used to follow trajectories/paths. The logic f
 
 ### Next steps
 
-Once the open loop response is good, you may begin adding feedback control. It is recommended you start tuning the drive velocity PID coefficients (`RUN_USING_ENCODER`) using `DriveVelocityPIDTuner` (if applicable). Then move on to the follower tunables with `FollowerPIDTuner`.
+Once the open loop response is good, you may begin adding positional feedback control using `FollowerPIDTuner`.
 
 ## Elevator Getting Started
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java

@@ -39,4 +39,8 @@ private DriveConstants() {
     public static double encoderTicksToInches(int ticks) {
         return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
     }
+
+    public static double rpmToVelocity(double rpm) {
+        return rpm * DriveConstants.GEAR_RATIO * 2 * Math.PI * DriveConstants.WHEEL_RADIUS / 60.0;
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDriveREV.java

@@ -47,6 +47,9 @@ public SampleMecanumDriveREV(HardwareMap hardwareMap) {
         }
 
         // TODO: reverse any motors using DcMotor.setDirection()
+
+        // TODO: set the tuned coefficients from DriveVelocityPIDTuner if using RUN_USING_ENCODER
+        // setPIDCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, ...);
     }
 
     @Override

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDriveREV.java

@@ -48,6 +48,9 @@ public SampleTankDriveREV(HardwareMap hardwareMap) {
         }
 
         // TODO: reverse any motors using DcMotor.setDirection()
+
+        // TODO: set the tuned coefficients from DriveVelocityPIDTuner if using RUN_USING_ENCODER
+        // setPIDCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, ...);
     }
 
     @Override

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveFFTuningOpMode.java

@@ -29,8 +29,6 @@
 @Config
 @Autonomous
 public class DriveFFTuningOpMode extends LinearOpMode {
-    private static final double EPSILON = 1e-2;
-
     public static final double MAX_POWER = 0.7;
     public static final double DISTANCE = 100;
 
@@ -86,8 +84,7 @@ public void runOpMode() throws InterruptedException {
         telemetry.log().add("Running...");
         telemetry.update();
 
-        double maxRpm = DriveConstants.MOTOR_CONFIG.getMaxRPM();
-        double maxVel = maxRpm * DriveConstants.GEAR_RATIO * 2 * Math.PI * DriveConstants.WHEEL_RADIUS / 60.0;
+        double maxVel = DriveConstants.rpmToVelocity(DriveConstants.MOTOR_CONFIG.getMaxRPM());
         double finalVel = MAX_POWER * maxVel;
         double accel = (finalVel * finalVel) / (2.0 * DISTANCE);
         double rampTime = Math.sqrt(2.0 * DISTANCE / accel);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java

@@ -38,6 +38,12 @@ public class DriveVelocityPIDTuner extends LinearOpMode {
     public static PIDCoefficients MOTOR_PID = new PIDCoefficients();
     public static double DISTANCE = 72;
 
+    /*
+     * If true, the kV value is computed from the free speed determined by the manufacturer (likely
+     * an overestimate of the actual value. If false, the value from DriveConstants.kV is used.
+     */
+    public static boolean USE_THEORETICAL_KV = true;
+
     @Override
     public void runOpMode() {
         FtcDashboard dashboard = FtcDashboard.getInstance();
@@ -68,6 +74,9 @@ public void runOpMode() {
         double lastTimestamp = 0;
         double profileStartTimestamp = clock.seconds();
 
+        double maxVel = DriveConstants.rpmToVelocity(DriveConstants.MOTOR_CONFIG.getMaxRPM());
+        double kV = USE_THEORETICAL_KV ? (1.0 / maxVel) : DriveConstants.kV;
+
         while (!isStopRequested()) {
             // update the coefficients if necessary
             if (!pidEquals(currentCoeffs, MOTOR_PID)) {
@@ -90,7 +99,7 @@ public void runOpMode() {
                 profileStartTimestamp = clock.seconds();
             }
             MotionState motionState = activeProfile.get(profileTime);
-            double targetPower = DriveConstants.kV * motionState.getV();
+            double targetPower = kV * motionState.getV();
             drive.setVelocity(new Pose2d(targetPower, 0, 0));
 
             List<Double> wheelPositions = drive.getWheelPositions();