Merge branch 'master' into jhh/migrating-to-yaml-syntax

Author: mwitcpalek

src/main/java/frc/team2767/deepspace/command/TeleOpDriveCommand.java

@@ -7,6 +7,7 @@
 import frc.team2767.deepspace.control.DriverControls;
 import frc.team2767.deepspace.subsystem.DriveSubsystem;
 import frc.team2767.deepspace.subsystem.VisionSubsystem;
+import frc.team2767.deepspace.util.VectorRateLimit;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
 import org.strykeforce.thirdcoast.util.ExpoScale;
@@ -18,18 +19,21 @@ public final class TeleOpDriveCommand extends Command {
   private static final double DEADBAND = 0.05;
   private static final double YAW_EXPO = 0.65; // 0.5
   private static final double DRIVE_EXPO = 0.65; // 0.5
+  private static final double VECTOR_LIMIT = 0.05;
   private static final double kP = 0.1;
   private static final double MAX_FWD_STR = 0.7;
   private static final double MAX_YAW = 0.6;
   private static DriverControls controls;
   private final Logger logger = LoggerFactory.getLogger(this.getClass());
   private final ExpoScale yawExpo;
   private final ExpoScale driveExpo;
+  private final VectorRateLimit vectorLimit;
 
   public TeleOpDriveCommand() {
     requires(DRIVE);
     this.yawExpo = new ExpoScale(DEADBAND, YAW_EXPO);
     this.driveExpo = new ExpoScale(DEADBAND, DRIVE_EXPO);
+    this.vectorLimit = new VectorRateLimit(VECTOR_LIMIT);
   }
 
   @Override
@@ -46,11 +50,16 @@ protected void execute() {
     double forward = driveExpo.apply(controls.getForward());
     double strafe = driveExpo.apply(controls.getStrafe());
 
+    // double forward = controls.getForward();
+    // double strafe = controls.getStrafe();
+
     forward = maximum(forward, MAX_FWD_STR);
     strafe = maximum(strafe, MAX_FWD_STR);
     yaw = maximum(yaw, MAX_YAW);
 
-    DRIVE.drive(forward, strafe, yaw);
+    double[] output = vectorLimit.apply(forward, strafe);
+
+    DRIVE.drive(output[0], output[1], yaw);
   }
 
   @Override

src/main/java/frc/team2767/deepspace/subsystem/VisionSubsystem.java

@@ -29,8 +29,8 @@ public class VisionSubsystem extends Subsystem implements Measurable {
   private static final double CAMERA_X = 3.5;
   private static final double CAMERA_Y_LEFT = -13.5;
   private static final double CAMERA_Y_RIGHT = 13.5;
-  private static final double GLUE_CORRECTION_FACTOR_RIGHT = -1.8181; // -2.21
-  private static final double GLUE_CORRECTION_FACTOR_LEFT = 0.8556; // 0.6417
+  private static final double GLUE_CORRECTION_FACTOR_RIGHT = -1.9251; // -1.8181
+  private static final double GLUE_CORRECTION_FACTOR_LEFT = 0.9625; // 0.8556
   private static final double CAMERA_DEGREES_PER_PIXEL_ADJUSTMENT_RIGHT =
       1.0; // 1.0 is zero value 0.85
   private static final double CAMERA_DEGREES_PER_PIXEL_ADJUSTMENT_LEFT =
@@ -41,10 +41,10 @@ public class VisionSubsystem extends Subsystem implements Measurable {
   private static final double CAMERA_RANGE_OFFSET_RIGHT = -5.7751; // -5.02
   private static final double CAMERA_RANGE_SLOPE_LEFT = 1.0437; // 1.061
   private static final double CAMERA_RANGE_OFFSET_LEFT = -4.3828; // -4.92
-  // NEGATIVE = TOWARDS FIELD LEFT (this one was negative)
+  // NEGATIVE = TOWARDS FIELD LEFT (this one was negative) from driver station perspective
   private static final double STRAFE_CORRECTION_RIGHT =
-      -0.5; // -1.0 // NEGATIVE TO FIELD LEFT FOR THIS ONE?
-  private static final double STRAFE_CORRECTION_LEFT = 0.5; // was positive
+      -0.75; // -1.0 // NEGATIVE TO FIELD LEFT FOR THIS ONE
+  private static final double STRAFE_CORRECTION_LEFT = 0; // was positive
   private static final String RANGE = "RANGE";
   private static final String CORRECTED_BEARING = "CORRECTED_BEARING";
   private static final String LIGHT_STATE = "LIGHT_STATE";

src/main/java/frc/team2767/deepspace/util/VectorRateLimit.java

@@ -0,0 +1,45 @@
+package frc.team2767.deepspace.util;
+
+public class VectorRateLimit {
+  private static final Double CLOSE_ENOUGH = 0.0001;
+  private final double rateLimit;
+
+  private double maxFwdStp;
+  private double maxStrStp;
+
+  private double curVel;
+
+  private double[] output = {0, 0};
+
+  public VectorRateLimit(double rateLimit) {
+    this.rateLimit = rateLimit;
+  }
+
+  public double[] apply(double forward, double strafe) {
+    curVel = Math.hypot(forward, strafe);
+
+    // Calculate Steps
+    maxFwdStp = Math.sin(Math.atan2(forward, strafe));
+    maxStrStp = Math.cos(Math.atan2(forward, strafe));
+
+    if (Math.abs(maxFwdStp - 0) < CLOSE_ENOUGH) {
+      maxFwdStp = rateLimit * (maxFwdStp - Math.sin(Math.atan2(output[0], output[1])));
+    } else maxFwdStp = rateLimit * maxFwdStp;
+
+    if (Math.abs(maxStrStp - 0) < CLOSE_ENOUGH) {
+      maxStrStp = rateLimit * (maxStrStp - Math.cos(Math.atan2(output[0], output[1])));
+    } else maxStrStp = rateLimit * maxStrStp;
+
+    // Calculate Forward Component
+    if (Math.abs(forward - output[0]) > Math.abs(maxFwdStp)) {
+      output[0] = output[0] + Math.copySign(maxFwdStp, forward - output[0]);
+    } else output[0] = forward;
+
+    // Calculate Strafe Component
+    if (Math.abs(strafe - output[1]) > Math.abs(maxStrStp)) {
+      output[1] = output[1] + Math.copySign(maxStrStp, strafe - output[1]);
+    } else output[1] = strafe;
+
+    return output;
+  }
+}

src/test/java/frc/team2767/deepspace/util/VectorRateLimitTest.java

@@ -0,0 +1,36 @@
+package frc.team2767.deepspace.util;
+
+import static org.assertj.core.api.Assertions.assertThat;
+
+import org.assertj.core.data.Percentage;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.CsvFileSource;
+
+class VectorRateLimitTest {
+
+  private static double[] fwdOutput;
+  private static double[] strOutput;
+  private static final double TOLERANCE = 1;
+
+  @ParameterizedTest
+  @CsvFileSource(resources = "/sticks.csv", numLinesToSkip = 1)
+  void getFwd(
+      double rateLimit, double forward, double strafe, double expectedFwd, double expectedStr) {
+    VectorRateLimit vectorLimit = new VectorRateLimit(rateLimit);
+
+    fwdOutput = vectorLimit.apply(forward, strafe);
+
+    assertThat(fwdOutput[0]).isCloseTo(expectedFwd, Percentage.withPercentage(TOLERANCE));
+  }
+
+  @ParameterizedTest
+  @CsvFileSource(resources = "/sticks.csv", numLinesToSkip = 1)
+  void getStr(
+      double rateLimit, double forward, double strafe, double expectedFwd, double expectedStr) {
+    VectorRateLimit vectorRateLimit = new VectorRateLimit(rateLimit);
+
+    strOutput = vectorRateLimit.apply(forward, strafe);
+
+    assertThat(strOutput[1]).isCloseTo(expectedStr, Percentage.withPercentage(TOLERANCE));
+  }
+}

src/test/resources/sticks.csv

@@ -0,0 +1,6 @@
+rateLimit,forward,strafe,expectedFwd,expectedStr
+0.4,1,0,0.4,0
+0.2,1,1,0.1414,0.1414
+0.2,1,0.2,0.1961,0.0392
+0.2,-1,0,-0.2,0
+0.2,-1,0.5,-0.1789,0.0894