More fixes

lasarobotics · Dec 23, 2024 · 2b00785 · 2b00785
1 parent 6cdd9ac
commit 2b00785
Show file tree

Hide file tree

Showing 27 changed files with 400 additions and 171 deletions.
diff --git a/src/main/java/org/lasarobotics/drive/RotatePIDController.java b/src/main/java/org/lasarobotics/drive/RotatePIDController.java
@@ -15,6 +15,7 @@
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Dimensionless;
 
 /** Rotate PID controller */
 public class RotatePIDController {
@@ -24,8 +25,8 @@ public class RotatePIDController {
 
   private HashMap<Double, Double> m_rotateInputMap = new HashMap<Double, Double>();
   private PIDController m_pidController;
-  private double m_rotateScalar;
-  private double m_lookAhead;
+  private Angle m_rotateScalar;
+  private int m_lookAhead;
   private double m_deadband;
   private double m_rotateRequest;
   private boolean m_isRotating;
@@ -34,17 +35,20 @@ public class RotatePIDController {
    * Create an instance of RotatePIDController
    * @param rotateInputCurve Rotate input curve
    * @param pidf PID constants
-   * @param rotateScalar Value to turn input by (degrees)
+   * @param rotateScalar Value to scale rotate input by
    * @param deadband Controller deadband
    * @param lookAhead Number of loops to look ahead by
    */
-  public RotatePIDController(PolynomialSplineFunction rotateInputCurve, PIDConstants pidf, double rotateScalar, double deadband, double lookAhead) {
+  public RotatePIDController(PolynomialSplineFunction rotateInputCurve, PIDConstants pidf, Angle rotateScalar, Dimensionless deadband, int lookAhead) {
     this.m_pidController = new PIDController(pidf.kP, 0.0, pidf.kD, pidf.period.in(Units.Seconds));
     this.m_rotateScalar = rotateScalar;
-    this.m_deadband = MathUtil.clamp(deadband, MIN_DEADBAND, MAX_DEADBAND);
+    this.m_deadband = MathUtil.clamp(deadband.in(Units.Percent), MIN_DEADBAND, MAX_DEADBAND);
     this.m_lookAhead = lookAhead;
     this.m_isRotating = false;
 
+    // Enable PID position wrapping
+    m_pidController.enableContinuousInput(0.0, 360.0);
+
     // Fill turn input hashmap
     for (int i = 0; i <= 1000; i++) {
       double key = (double)i / 1000;
@@ -75,7 +79,7 @@ public AngularVelocity calculate(Angle currentAngle, AngularVelocity rotateRate,
       m_rotateRequest = Math.copySign(Math.floor(Math.abs(m_rotateRequest) * 1000) / 1000, m_rotateRequest) + 0.0;
       double scaledTurnRequest = m_rotateInputMap.get(m_rotateRequest);
       // Add delta to setpoint scaled by factor
-      m_pidController.setSetpoint(currentAngle.in(Units.Degrees) + (scaledTurnRequest * m_rotateScalar));
+      m_pidController.setSetpoint(currentAngle.in(Units.Degrees) + m_rotateScalar.times(scaledTurnRequest).in(Units.Degrees));
       m_isRotating = true;
     } else {
       // When rotation is complete, set setpoint to current angle

diff --git a/src/main/java/org/lasarobotics/drive/ThrottleMap.java b/src/main/java/org/lasarobotics/drive/ThrottleMap.java
@@ -10,6 +10,7 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Dimensionless;
 import edu.wpi.first.units.measure.LinearVelocity;
 
 /** Throttle map */
@@ -27,8 +28,8 @@ public class ThrottleMap {
    * @param maxLinearSpeed Maximum linear speed of robot
    * @param deadband Deadband for controller input [+0.001, +0.2]
    */
-  public ThrottleMap(PolynomialSplineFunction throttleInputCurve, LinearVelocity maxLinearSpeed, double deadband) {
-    this.m_deadband = MathUtil.clamp(deadband, MIN_DEADBAND, MAX_DEADBAND);
+  public ThrottleMap(PolynomialSplineFunction throttleInputCurve, LinearVelocity maxLinearSpeed, Dimensionless deadband) {
+    this.m_deadband = MathUtil.clamp(deadband.in(Units.Percent), MIN_DEADBAND, MAX_DEADBAND);
 
     // Fill throttle input hashmap
     for (int i = 0; i <= 1000; i++) {

diff --git a/src/main/java/org/lasarobotics/drive/swerve/DriveWheel.java b/src/main/java/org/lasarobotics/drive/swerve/DriveWheel.java
@@ -12,9 +12,23 @@ public class DriveWheel {
   public final Dimensionless staticCoF;
   public final Dimensionless dynamicCoF;
 
-  public DriveWheel(Distance diameter, Dimensionless staticCoF, Dimensionless dynamicCoF) {
+  private DriveWheel(Distance diameter, Dimensionless staticCoF, Dimensionless dynamicCoF) {
     this.diameter = diameter;
     this.staticCoF = staticCoF;
     this.dynamicCoF = dynamicCoF;
   }
+
+  /**
+   * Create a swerve drive wheel
+   * @param diameter Diameter of wheel
+   * @param staticCoF Static coefficient of friction
+   * @param dynamicCoF Dynamic coefficient of friction
+   * @return
+   */
+  public static DriveWheel create(Distance diameter, Dimensionless staticCoF, Dimensionless dynamicCoF) {
+    if (dynamicCoF.gt(staticCoF))
+      throw new IllegalArgumentException("Static CoF must be higher than dynamic CoF!");
+
+    return new DriveWheel(diameter, staticCoF, dynamicCoF);
+  }
 }
diff --git a/src/main/java/org/lasarobotics/drive/swerve/README.md b/src/main/java/org/lasarobotics/drive/swerve/README.md
@@ -1,15 +1,42 @@
 # REV Swerve
 
-In order to use Spark motor controllers, you MUST have a PWM absolute encoder connected to the Spark that controls the rotation motor.
+In order to use Spark motor controllers, you MUST have a PWM absolute encoder directly connected to the Spark that controls the rotation motor.
 
 Options include:
  * [REV Through Bore Encoder](https://www.revrobotics.com/rev-11-1271/)
  * [Redux Robotics HELIUM Canandcoder (in PWM mode)](https://shop.reduxrobotics.com/helium-canandmag/)
  * [Thriftybot Absolute Magnetic Encoder](https://www.thethriftybot.com/products/thrifty-absolute-magnetic-encoder)
 
+ To create an instance of a module, use the helper class for your module, and call the `create()` method for REV.
+
+ For example, here's how to create a MAXSwerve module:
+ ```
+REVSwerveModule lFrontModule = MAXSwerveModule.create(
+   REVSwerveModule.initializeHardware(
+     Constants.DriveHardware.LEFT_FRONT_DRIVE_MOTOR_ID,
+     Constants.DriveHardware.LEFT_FRONT_ROTATE_MOTOR_ID,
+     MotorKind.NEO_VORTEX,
+     MotorKind.NEO_550
+   ),
+   SwerveModule.Location.LeftFront,
+   MAXSwerveModule.GearRatio.L3,
+   DriveWheel.create(Units.Inches.of(3.0), Units.Value.of(0.9), Units.Value.of(0.8)),
+   PIDConstants.of(0.3, 0.0, 0.001, 0.0, 0.0),
+   FFConstants.of(0.2, 0.0, 0.0, 0.5),
+   PIDConstants.of(2.0, 0.0, 0.1, 0.0, 0.0),
+   FFConstants.of(0.2, 0.0, 0.0, 0.01),
+   Units.Percent.of(8.0),
+   Units.Pounds.of(135.0),
+   Units.Meters.of(0.5588),
+   Units.Meters.of(0.5588),
+   Units.Seconds.of(3.0),
+   Units.Amps.of(60.0)
+);
+```
+
  # CTRE Swerve
 
- At the current time, the CTRE CANCoder is the ONLY encoder supported.
+ At the current time, the CTRE CANCoder is the ONLY absolute encoder supported.
 
 ## MAXSwerve
 
@@ -24,7 +51,7 @@ Drive PID: 0.3, 0.0, 0.001
 
 Drive kS: 0.2
 
-Drive kV: 12 / (calculated max speed in meters per second)
+Drive kV: 0.0
 
 Drive kA: 0.5
 

diff --git a/src/main/java/org/lasarobotics/drive/swerve/child/MAXSwerveModule.java b/src/main/java/org/lasarobotics/drive/swerve/child/MAXSwerveModule.java
@@ -22,7 +22,7 @@
 import edu.wpi.first.units.measure.Time;
 
 /** Helper class to create an instance of MAXSwerveModule */
-public class MAXSwerveModule {
+public abstract class MAXSwerveModule {
 
   /**
    * MAXSwerve gear ratios
@@ -70,6 +70,11 @@ public double getRotateRatio() {
     Map.entry(SwerveModule.Location.RightRear, Units.Radians.of(Math.PI / 2))
   );
 
+  /**
+   * Do not allow direct instantiation
+   */
+  private MAXSwerveModule() {}
+
   /**
    * Create an instance of a REV MAXSwerve module
    * @param swerveHardware Hardware devices required by swerve module
@@ -100,8 +105,10 @@ public static REVSwerveModule create(REVSwerveModule.Hardware swerveHardware,
 
 
     return new REVSwerveModule(swerveHardware,
-                               SwerveModule.MountOrientation.INVERTED, // Used to force encoder inversion
-                               location, gearRatio,
+                               location,
+                               SwerveModule.MountOrientation.STANDARD,
+                               SwerveModule.MountOrientation.INVERTED,
+                               gearRatio,
                                driveWheel, ZERO_OFFSET.get(location),
                                drivePID, driveFF,
                                rotatePID, rotateFF,

diff --git a/src/main/java/org/lasarobotics/drive/swerve/child/MK4Module.java b/src/main/java/org/lasarobotics/drive/swerve/child/MK4Module.java
@@ -21,7 +21,7 @@
 import edu.wpi.first.units.measure.Time;
 
 /** Helper class to create an instance of MK4Module */
-public class MK4Module {
+public abstract class MK4Module {
 
   /**
    * MK4 gear ratios
@@ -56,6 +56,11 @@ public double getRotateRatio() {
 
   private static final Angle ZERO_OFFSET = Units.Radians.zero();
 
+  /**
+   * Do not allow direct instantiation
+   */
+  private MK4Module() {}
+
   /**
    * Create an instance of a SDS MK4 module powered by REV Spark motor controllers
    * @param swerveHardware Hardware devices required by swerve module
@@ -85,8 +90,9 @@ public static REVSwerveModule create(REVSwerveModule.Hardware swerveHardware,
       throw new IllegalArgumentException("MK4 rotate motor cannot be a NEO 550!");
 
     return new REVSwerveModule(swerveHardware,
-                               SwerveModule.MountOrientation.STANDARD,
                                location,
+                               SwerveModule.MountOrientation.STANDARD,
+                               SwerveModule.MountOrientation.STANDARD,
                                gearRatio,
                                driveWheel, ZERO_OFFSET,
                                drivePID, driveFF,
@@ -122,8 +128,9 @@ public static CTRESwerveModule create(CTRESwerveModule.Hardware swerveHardware,
                                         Distance wheelbase, Distance trackWidth,
                                         Time autoLockTime, Current driveCurrentLimit) {
     return new CTRESwerveModule(swerveHardware,
-                                SwerveModule.MountOrientation.STANDARD,
                                 location,
+                                SwerveModule.MountOrientation.STANDARD,
+                                SwerveModule.MountOrientation.STANDARD,
                                 gearRatio,
                                 driveWheel, ZERO_OFFSET,
                                 drivePID, driveFF,

diff --git a/src/main/java/org/lasarobotics/drive/swerve/child/MK4cModule.java b/src/main/java/org/lasarobotics/drive/swerve/child/MK4cModule.java
@@ -21,7 +21,7 @@
 import edu.wpi.first.units.measure.Time;
 
 /** Helper class to create an instance of MK4cModule */
-public class MK4cModule {
+public abstract class MK4cModule {
 
   /**
    * MK4c gear ratios
@@ -54,6 +54,11 @@ public double getRotateRatio() {
 
   private static final Angle ZERO_OFFSET = Units.Radians.zero();
 
+  /**
+   * Do not allow direct instantiation
+   */
+  private MK4cModule() {}
+
   /**
    * Create an instance of a SDS MK4c module powered by REV Spark motor controllers
    * @param swerveHardware Hardware devices required by swerve module
@@ -83,8 +88,9 @@ public static REVSwerveModule create(REVSwerveModule.Hardware swerveHardware,
       throw new IllegalArgumentException("MK4c rotate motor cannot be a NEO 550!");
 
     return new REVSwerveModule(swerveHardware,
-                               SwerveModule.MountOrientation.STANDARD,
                                location,
+                               SwerveModule.MountOrientation.STANDARD,
+                               SwerveModule.MountOrientation.STANDARD,
                                gearRatio,
                                driveWheel, ZERO_OFFSET,
                                drivePID, driveFF,
@@ -120,8 +126,9 @@ public static CTRESwerveModule create(CTRESwerveModule.Hardware swerveHardware,
                                         Distance wheelbase, Distance trackWidth,
                                         Time autoLockTime, Current driveCurrentLimit) {
     return new CTRESwerveModule(swerveHardware,
-                                SwerveModule.MountOrientation.STANDARD,
                                 location,
+                                SwerveModule.MountOrientation.STANDARD,
+                                SwerveModule.MountOrientation.STANDARD,
                                 gearRatio,
                                 driveWheel, ZERO_OFFSET,
                                 drivePID, driveFF,

diff --git a/src/main/java/org/lasarobotics/drive/swerve/child/MK4iModule.java b/src/main/java/org/lasarobotics/drive/swerve/child/MK4iModule.java
@@ -21,7 +21,7 @@
 import edu.wpi.first.units.measure.Time;
 
 /** Helper class to create an instance of MK4iModule */
-public class MK4iModule {
+public abstract class MK4iModule {
 
   /**
    * MK4i gear ratios
@@ -56,6 +56,11 @@ public double getRotateRatio() {
 
   private static final Angle ZERO_OFFSET = Units.Radians.zero();
 
+  /**
+   * Do not allow direct instantiation
+   */
+  private MK4iModule() {}
+
   /**
    * Create an instance of a SDS MK4i module powered by REV Spark motor controllers
    * @param swerveHardware Hardware devices required by swerve module
@@ -85,8 +90,9 @@ public static REVSwerveModule create(REVSwerveModule.Hardware swerveHardware,
       throw new IllegalArgumentException("MK4i rotate motor cannot be a NEO 550!");
 
     return new REVSwerveModule(swerveHardware,
-                               SwerveModule.MountOrientation.INVERTED,
                                location,
+                               SwerveModule.MountOrientation.INVERTED,
+                               SwerveModule.MountOrientation.STANDARD,
                                gearRatio,
                                driveWheel, ZERO_OFFSET,
                                drivePID, driveFF,
@@ -122,8 +128,9 @@ public static CTRESwerveModule create(CTRESwerveModule.Hardware swerveHardware,
                                         Distance wheelbase, Distance trackWidth,
                                         Time autoLockTime, Current driveCurrentLimit) {
     return new CTRESwerveModule(swerveHardware,
-                                SwerveModule.MountOrientation.INVERTED,
                                 location,
+                                SwerveModule.MountOrientation.INVERTED,
+                                SwerveModule.MountOrientation.STANDARD,
                                 gearRatio,
                                 driveWheel, ZERO_OFFSET,
                                 drivePID, driveFF,

diff --git a/src/main/java/org/lasarobotics/drive/swerve/child/SwerveX2Module.java b/src/main/java/org/lasarobotics/drive/swerve/child/SwerveX2Module.java
@@ -21,7 +21,7 @@
 import edu.wpi.first.units.measure.Time;
 
 /** Helper class to create an instance of SwerveX2Module */
-public class SwerveX2Module {
+public abstract class SwerveX2Module {
 
   /**
    * Swerve X2 gear ratios
@@ -72,9 +72,15 @@ public double getRotateRatio() {
 
   private static final Angle ZERO_OFFSET = Units.Radians.of(Math.PI / 4);
 
+  /**
+   * Do not allow direct instantiation
+   */
+  private SwerveX2Module() {}
+
   /**
    * Create an instance of a WCP Swerve X2 module powered by REV Spark motor controllers
    * @param swerveHardware Hardware devices required by swerve module
+   * @param orientation Motor mount orientation
    * @param location Location of module
    * @param gearRatio Gear ratio for module
    * @param driveWheel Wheel installed in swerve module
@@ -91,6 +97,7 @@ public double getRotateRatio() {
    */
   public static REVSwerveModule create(REVSwerveModule.Hardware swerveHardware,
                                        SwerveModule.Location location,
+                                       SwerveModule.MountOrientation orientation,
                                        SwerveX2Module.GearRatio gearRatio, DriveWheel driveWheel,
                                        PIDConstants drivePID, FFConstants driveFF,
                                        PIDConstants rotatePID, FFConstants rotateFF,
@@ -101,10 +108,11 @@ public static REVSwerveModule create(REVSwerveModule.Hardware swerveHardware,
       throw new IllegalArgumentException("Swerve X2 rotate motor cannot be a NEO 550!");
 
     return new REVSwerveModule(swerveHardware,
-                               SwerveModule.MountOrientation.STANDARD,
                                location,
-                               gearRatio,
-                               driveWheel, ZERO_OFFSET,
+                               orientation,
+                               SwerveModule.MountOrientation.STANDARD,
+                               gearRatio, driveWheel,
+                               ZERO_OFFSET,
                                drivePID, driveFF,
                                rotatePID, rotateFF,
                                slipRatio, mass,
@@ -116,6 +124,7 @@ public static REVSwerveModule create(REVSwerveModule.Hardware swerveHardware,
    * Create an instance of a WCP Swerve X2 module powered by CTRE TalonFX motor controllers
    * @param swerveHardware Hardware devices required by swerve module
    * @param location Location of module
+   * @param orientation Motor mount orientation
    * @param gearRatio Gear ratio for module
    * @param driveWheel Wheel installed in swerve module
    * @param drivePID Drive motor PID gains
@@ -131,17 +140,19 @@ public static REVSwerveModule create(REVSwerveModule.Hardware swerveHardware,
    */
   public static CTRESwerveModule create(CTRESwerveModule.Hardware swerveHardware,
                                         SwerveModule.Location location,
+                                        SwerveModule.MountOrientation orientation,
                                         SwerveX2Module.GearRatio gearRatio, DriveWheel driveWheel,
                                         PIDConstants drivePID, FFConstants driveFF,
                                         PIDConstants rotatePID, FFConstants rotateFF,
                                         Dimensionless slipRatio, Mass mass,
                                         Distance wheelbase, Distance trackWidth,
                                         Time autoLockTime, Current driveCurrentLimit) {
     return new CTRESwerveModule(swerveHardware,
-                                SwerveModule.MountOrientation.STANDARD,
                                 location,
-                                gearRatio,
-                                driveWheel, ZERO_OFFSET,
+                                orientation,
+                                SwerveModule.MountOrientation.STANDARD,
+                                gearRatio, driveWheel,
+                                ZERO_OFFSET,
                                 drivePID, driveFF,
                                 rotatePID, rotateFF,
                                 slipRatio, mass,