Merge pull request #50 from TeamFrozenCodeFTC/dev

Reverse Power for PredictiveBrakingTuner

Author: junkjunk123

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java

@@ -77,6 +77,7 @@ public Tuning() {
                 p.add("Line", Line::new);
                 p.add("Triangle", Triangle::new);
                 p.add("Circle", Circle::new);
+                p.add("Line90DegreeTurn", Line90DegreeTurn::new);
             });
         });
     }
@@ -725,7 +726,7 @@ public void loop() {
 /**
  * This is the Predictive Braking Tuner. It runs the robot forward and backward at various power
  * levels, recording the robot’s velocity and position immediately before braking. The motors are
- * then set to zero-power brake mode, which represents the fastest theoretical braking the robot
+ * then set to a reverse power, which represents the fastest theoretical braking the robot
  * can achieve. Once the robot comes to a complete stop, the tuner measures the stopping distance.
  * Using the collected data, it generates a velocity-vs-stopping-distance graph and fits a
  * quadratic curve to model the braking behavior.
@@ -736,10 +737,10 @@ public void loop() {
  */
 class PredictiveBrakingTuner extends OpMode {
     private static final double[] TEST_POWERS =
-            {1, 1, 1, 0.9, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2};
-
-    private static final int DRIVE_TIME_MS = 2000;
-    private static final int BRAKE_WAIT_MS = 1000;
+        {1, 1, 1, 0.9, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2};
+    private static final double BRAKING_POWER = -0.2;
+    
+    private static final int DRIVE_TIME_MS = 1000;
 
     private enum State {
         START_MOVE,
@@ -749,37 +750,34 @@ private enum State {
         RECORD,
         DONE
     }
-
+    
     private static class BrakeRecord {
         double timeMs;
         Pose pose;
         double velocity;
-
+        
         BrakeRecord(double timeMs, Pose pose, double velocity) {
             this.timeMs = timeMs;
             this.pose = pose;
             this.velocity = velocity;
         }
     }
-
-    private double lastDriveSign = 1.0;
-    private static final int BRAKE_PULSE_MS = 150;
-
+    
     private State state = State.START_MOVE;
-
+    
     private final ElapsedTime timer = new ElapsedTime();
-
+    
     private int iteration = 0;
-
+    
     private Vector startPosition;
     private double measuredVelocity;
-
+    
     private final List<double[]> velocityToBrakingDistance = new ArrayList<>();
     private final List<BrakeRecord> brakeData = new ArrayList<>();
-
+    
     @Override
     public void init() {}
-
+    
     @Override
     public void init_loop() {
         telemetryM.debug("The robot will move forwards and backwards starting at max speed and slowing down.");
@@ -791,47 +789,43 @@ public void init_loop() {
         follower.update();
         drawCurrent();
     }
-
+    
     @Override
     public void start() {
         timer.reset();
         follower.update();
         follower.startTeleOpDrive(true);
     }
-
+    
     @SuppressLint("DefaultLocale")
     @Override
     public void loop() {
         follower.update();
-
+        
         if (gamepad1.b) {
             stopRobot();
             requestOpModeStop();
             return;
         }
-
+        
+        double direction = (iteration % 2 == 0) ? 1 : -1;
+        
         switch (state) {
             case START_MOVE: {
                 if (iteration >= TEST_POWERS.length) {
                     state = State.DONE;
                     break;
                 }
-
+                
                 double currentPower = TEST_POWERS[iteration];
                 follower.setMaxPower(currentPower);
-                if (iteration % 2 != 0) {
-                    lastDriveSign = -1.0;
-                    follower.setTeleOpDrive(-1, 0, 0, true);
-                } else {
-                    lastDriveSign = 1.0;
-                    follower.setTeleOpDrive(1, 0, 0, true);
-                }
-
+                follower.setTeleOpDrive(direction, 0, 0, true);
+       
                 timer.reset();
                 state = State.WAIT_DRIVE_TIME;
                 break;
             }
-
+            
             case WAIT_DRIVE_TIME: {
                 if (timer.milliseconds() >= DRIVE_TIME_MS) {
                     measuredVelocity = follower.getVelocity().getMagnitude();
@@ -840,57 +834,55 @@ public void loop() {
                 }
                 break;
             }
-
+            
             case APPLY_BRAKE: {
-                double reversePower = -lastDriveSign * follower.constants.predictiveBrakingCoefficients.maximumBrakingPower;
-                follower.setTeleOpDrive(reversePower, 0, 0, true);
+                follower.setTeleOpDrive(BRAKING_POWER * direction, 0, 0, true);
 
                 timer.reset();
                 state = State.WAIT_BRAKE_TIME;
                 break;
             }
-
+            
             case WAIT_BRAKE_TIME: {
                 double t = timer.milliseconds();
                 Pose currentPose = follower.getPose();
                 double currentVelocity = follower.getVelocity().getMagnitude();
-
+                
                 brakeData.add(new BrakeRecord(t, currentPose, currentVelocity));
-
-                if (t >= BRAKE_PULSE_MS) {
-                    stopRobot();
-                }
-
-                if (timer.milliseconds() >= BRAKE_WAIT_MS || follower.getVelocity().getMagnitude() <= .05) {
+                
+                if (follower.getVelocity().dot(new Vector(direction,
+                                                          follower.getHeading())) <= 0) {
                     state = State.RECORD;
                 }
                 break;
             }
-
+            
             case RECORD: {
                 Vector endPosition = follower.getPose().getAsVector();
                 double brakingDistance = endPosition.minus(startPosition).getMagnitude();
-
+                
                 velocityToBrakingDistance.add(new double[]{measuredVelocity, brakingDistance});
-
+                
                 telemetryM.debug("Test " + iteration,
-                        String.format("v=%.3f  d=%.3f", measuredVelocity,
-                                brakingDistance));
+                                 String.format("v=%.3f  d=%.3f", measuredVelocity,
+                                               brakingDistance));
                 telemetryM.update(telemetry);
-
+                
                 iteration++;
                 state = State.START_MOVE;
-
+                
                 break;
             }
-
+            
             case DONE: {
+                stopRobot();
+                
                 double[] coefficients = quadraticFit(velocityToBrakingDistance);
-
+                
                 telemetryM.debug("Tuning Complete");
                 telemetryM.debug("Braking Profile:");
-                telemetryM.debug("kQuadraticFriction", coefficients[1]);
-                telemetryM.debug("kLinearBraking", coefficients[0]);
+                telemetryM.debug("kQuadratic", coefficients[1]);
+                telemetryM.debug("kLinear", coefficients[0]);
                 telemetryM.update(telemetry);
                 telemetryM.debug("Tuning Complete");
                 telemetryM.debug("Braking Profile:");
@@ -899,9 +891,9 @@ public void loop() {
                 for (BrakeRecord record : brakeData) {
                     Pose p = record.pose;
                     telemetryM.debug(String.format("t=%.0f ms, x=%.2f, y=%.2f, θ=%.2f, v=%.2f",
-                            record.timeMs, p.getX(), p.getY(),
-                            p.getHeading(),
-                            record.velocity));
+                                                   record.timeMs, p.getX(), p.getY(),
+                                                   p.getHeading(),
+                                                   record.velocity));
                 }
                 telemetryM.update();
                 break;
@@ -1182,6 +1174,48 @@ public void loop() {
     }
 }
 
+/**
+ * @author Jacob Ophoven - 18535 Frozen Code
+ */
+class Line90DegreeTurn extends OpMode {
+    @Override
+    public void init() {}
+    
+    /** This initializes the Follower and creates the forward and backward Paths. */
+    @Override
+    public void init_loop() {
+        telemetryM.debug("The robot will go forward 48 inches and then sideways to " +
+                             "the left 24 inches.");
+        telemetryM.update(telemetry);
+        follower.update();
+        drawCurrent();
+    }
+    
+    @Override
+    public void start() {
+        follower.activateAllPIDFs();
+        Path forwards = new Path(new BezierLine(new Pose(0, 0), new Pose(48, 0)));
+        forwards.setConstantHeadingInterpolation(0);
+        Path sideways = new Path(new BezierLine(new Pose(48, 0), new Pose(48,
+                                                                          24)));
+        sideways.setConstantHeadingInterpolation(0);
+        follower.followPath(new PathChain(forwards, sideways));
+    }
+    
+    /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */
+    @Override
+    public void loop() {
+        follower.update();
+        drawCurrentAndHistory();
+        
+        if (!follower.isBusy()) {
+            stopRobot();
+        }
+        
+        telemetryM.update(telemetry);
+    }
+}
+
 /**
  * This is the Centripetal Tuner OpMode. It runs the robot in a specified distance
  * forward and to the left. On reaching the end of the forward Path, the robot runs the backward