Add methods for retrieving wheel velocities for drive velocity PID tu…

…ner (fixes acmerobotics#33)
JackJones7 · Nov 3, 2019 · b679aa3 · b679aa3
1 parent fd925f6
commit b679aa3
Show file tree

Hide file tree

Showing 8 changed files with 120 additions and 22 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
@@ -57,7 +57,7 @@ public class DriveConstants {
     );
 
 
-    public static double encoderTicksToInches(int ticks) {
+    public static double encoderTicksToInches(double ticks) {
         return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
     }
 

diff --git a/...de/src/main/java/org/firstinspires/ftc/teamcode/drive/mecanum/SampleMecanumDriveBase.java b/...de/src/main/java/org/firstinspires/ftc/teamcode/drive/mecanum/SampleMecanumDriveBase.java
@@ -28,6 +28,9 @@
 import com.qualcomm.robotcore.hardware.DcMotor;
 import org.firstinspires.ftc.teamcode.util.DashboardUtil;
 
+import java.util.ArrayList;
+import java.util.List;
+
 /*
  * Base class with shared functionality for sample mecanum drives. All hardware-specific details are
  * handled in subclasses.
@@ -56,6 +59,9 @@ public enum Mode {
     private DriveConstraints constraints;
     private TrajectoryFollower follower;
 
+    private List<Double> lastWheelPositions;
+    private double lastTimestamp;
+
     public SampleMecanumDriveBase() {
         super(kV, kA, kStatic, TRACK_WIDTH);
 
@@ -197,6 +203,28 @@ public boolean isBusy() {
         return mode != Mode.IDLE;
     }
 
+    public List<Double> getWheelVelocities() {
+        List<Double> positions = getWheelPositions();
+        double currentTimestamp = clock.seconds();
+
+        List<Double> velocities = new ArrayList<>(positions.size());;
+        if (lastWheelPositions != null) {
+            double dt = currentTimestamp - lastTimestamp;
+            for (int i = 0; i < positions.size(); i++) {
+                velocities.add((positions.get(i) - lastWheelPositions.get(i)) / dt);
+            }
+        } else {
+            for (int i = 0; i < positions.size(); i++) {
+                velocities.add(0.0);
+            }
+        }
+
+        lastTimestamp = currentTimestamp;
+        lastWheelPositions = positions;
+
+        return velocities;
+    }
+
     public abstract PIDCoefficients getPIDCoefficients(DcMotor.RunMode runMode);
 
     public abstract void setPIDCoefficients(DcMotor.RunMode runMode, PIDCoefficients coefficients);

diff --git a/...ode/src/main/java/org/firstinspires/ftc/teamcode/drive/mecanum/SampleMecanumDriveREV.java b/...ode/src/main/java/org/firstinspires/ftc/teamcode/drive/mecanum/SampleMecanumDriveREV.java
@@ -90,6 +90,15 @@ public List<Double> getWheelPositions() {
         return wheelPositions;
     }
 
+    @Override
+    public List<Double> getWheelVelocities() {
+        List<Double> wheelVelocities = new ArrayList<>();
+        for (DcMotorEx motor : motors) {
+            wheelVelocities.add(encoderTicksToInches(motor.getVelocity()));
+        }
+        return wheelVelocities;
+    }
+
     @Override
     public void setMotorPowers(double v, double v1, double v2, double v3) {
         leftFront.setPower(v);

diff --git a/...ain/java/org/firstinspires/ftc/teamcode/drive/mecanum/SampleMecanumDriveREVOptimized.java b/...ain/java/org/firstinspires/ftc/teamcode/drive/mecanum/SampleMecanumDriveREVOptimized.java
@@ -6,6 +6,7 @@
 import com.acmerobotics.roadrunner.control.PIDCoefficients;
 import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.PIDFCoefficients;
 import java.util.ArrayList;
@@ -100,6 +101,21 @@ public List<Double> getWheelPositions() {
         return wheelPositions;
     }
 
+    @Override
+    public List<Double> getWheelVelocities() {
+        RevBulkData bulkData = hub.getBulkInputData();
+
+        if (bulkData == null) {
+            return Arrays.asList(0.0, 0.0, 0.0, 0.0);
+        }
+
+        List<Double> wheelVelocities = new ArrayList<>();
+        for (ExpansionHubMotor motor : motors) {
+            wheelVelocities.add(encoderTicksToInches(bulkData.getMotorVelocity(motor)));
+        }
+        return wheelVelocities;
+    }
+
     @Override
     public void setMotorPowers(double v, double v1, double v2, double v3) {
         leftFront.setPower(v);

diff --git a/...Code/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java b/...Code/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java
@@ -21,7 +21,6 @@
 import org.firstinspires.ftc.teamcode.drive.mecanum.SampleMecanumDriveBase;
 import org.firstinspires.ftc.teamcode.drive.mecanum.SampleMecanumDriveREV;
 
-import java.util.ArrayList;
 import java.util.List;
 
 import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
@@ -146,14 +145,10 @@ public void runOpMode() {
         MotionProfile activeProfile = generateProfile(true);
         double profileStart = clock.seconds();
 
-        List<Double> lastWheelPositions = null;
-        double lastTimestamp = 0;
 
         while (!isStopRequested()) {
             // calculate and set the motor power
             double profileTime = clock.seconds() - profileStart;
-            double dt = profileTime - lastTimestamp;
-            lastTimestamp = profileTime;
 
             if (profileTime > activeProfile.duration()) {
                 // generate a new profile
@@ -166,23 +161,15 @@ public void runOpMode() {
             double targetPower = kV * motionState.getV();
             drive.setDrivePower(new Pose2d(targetPower, 0, 0));
 
-            List<Double> wheelPositions = drive.getWheelPositions();
-            if (lastWheelPositions != null) {
-                // compute velocities
-                List<Double> syntheticVelocities = new ArrayList<>();
-                for (int i = 0; i < wheelPositions.size(); i++) {
-                    syntheticVelocities.add((wheelPositions.get(i) - lastWheelPositions.get(i)) / dt);
-                }
-
-                // update telemetry
-                telemetry.addData("targetVelocity", motionState.getV());
-                for (int i = 0; i < syntheticVelocities.size(); i++) {
-                    telemetry.addData("velocity" + i, syntheticVelocities.get(i));
-                    telemetry.addData("error" + i, motionState.getV() - syntheticVelocities.get(i));
-                }
-                telemetry.update();
+            List<Double> velocities = drive.getWheelVelocities();
+
+            // update telemetry
+            telemetry.addData("targetVelocity", motionState.getV());
+            for (int i = 0; i < velocities.size(); i++) {
+                telemetry.addData("velocity" + i, velocities.get(i));
+                telemetry.addData("error" + i, motionState.getV() - velocities.get(i));
             }
-            lastWheelPositions = wheelPositions;
+            telemetry.update();
         }
 
         removePidVariable();

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/tank/SampleTankDriveBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/tank/SampleTankDriveBase.java
@@ -28,6 +28,9 @@
 import com.qualcomm.robotcore.hardware.DcMotor;
 import org.firstinspires.ftc.teamcode.util.DashboardUtil;
 
+import java.util.ArrayList;
+import java.util.List;
+
 /*
  * Base class with shared functionality for sample tank drives. All hardware-specific details are
  * handled in subclasses.
@@ -57,6 +60,9 @@ public enum Mode {
     private DriveConstraints constraints;
     private TrajectoryFollower follower;
 
+    private List<Double> lastWheelPositions;
+    private double lastTimestamp;
+
     public SampleTankDriveBase() {
         super(kV, kA, kStatic, TRACK_WIDTH);
 
@@ -198,6 +204,28 @@ public boolean isBusy() {
         return mode != Mode.IDLE;
     }
 
+    public List<Double> getWheelVelocities() {
+        List<Double> positions = getWheelPositions();
+        double currentTimestamp = clock.seconds();
+
+        List<Double> velocities = new ArrayList<>(positions.size());;
+        if (lastWheelPositions != null) {
+            double dt = currentTimestamp - lastTimestamp;
+            for (int i = 0; i < positions.size(); i++) {
+                velocities.add((positions.get(i) - lastWheelPositions.get(i)) / dt);
+            }
+        } else {
+            for (int i = 0; i < positions.size(); i++) {
+                velocities.add(0.0);
+            }
+        }
+
+        lastTimestamp = currentTimestamp;
+        lastWheelPositions = positions;
+
+        return velocities;
+    }
+
     public abstract PIDCoefficients getPIDCoefficients(DcMotor.RunMode runMode);
 
     public abstract void setPIDCoefficients(DcMotor.RunMode runMode, PIDCoefficients coefficients);

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/tank/SampleTankDriveREV.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/tank/SampleTankDriveREV.java
@@ -94,6 +94,18 @@ public List<Double> getWheelPositions() {
         return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size());
     }
 
+    @Override
+    public List<Double> getWheelVelocities() {
+        double leftSum = 0, rightSum = 0;
+        for (DcMotorEx leftMotor : leftMotors) {
+            leftSum += encoderTicksToInches(leftMotor.getVelocity());
+        }
+        for (DcMotorEx rightMotor : rightMotors) {
+            rightSum += encoderTicksToInches(rightMotor.getVelocity());
+        }
+        return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size());
+    }
+
     @Override
     public void setMotorPowers(double v, double v1) {
         for (DcMotorEx leftMotor : leftMotors) {

diff --git a/.../src/main/java/org/firstinspires/ftc/teamcode/drive/tank/SampleTankDriveREVOptimized.java b/.../src/main/java/org/firstinspires/ftc/teamcode/drive/tank/SampleTankDriveREVOptimized.java
@@ -105,6 +105,24 @@ public List<Double> getWheelPositions() {
         return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size());
     }
 
+    @Override
+    public List<Double> getWheelVelocities() {
+        double leftSum = 0, rightSum = 0;
+        RevBulkData bulkData = hub.getBulkInputData();
+
+        if (bulkData == null) {
+            return Arrays.asList(0.0, 0.0);
+        }
+
+        for (DcMotorEx leftMotor : leftMotors) {
+            leftSum += encoderTicksToInches(bulkData.getMotorVelocity(leftMotor));
+        }
+        for (DcMotorEx rightMotor : rightMotors) {
+            rightSum += encoderTicksToInches(bulkData.getMotorVelocity(rightMotor));
+        }
+        return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size());
+    }
+
     @Override
     public void setMotorPowers(double v, double v1) {
         for (ExpansionHubMotor leftMotor : leftMotors) {
-Original file line number
+Diff line change
@@ Expand Up / @@ -57,7 +57,7 @@ public class DriveConstants { @@
         );
-        public static double encoderTicksToInches(int ticks) {
+        public static double encoderTicksToInches(double ticks) {
             return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
         }
@@ Expand Down @@