Updated safety's added new encoder

BroncBotz3481 · Sep 1, 2023 · a5aae87 · a5aae87
1 parent 8711ec2
commit a5aae87
Show file tree

Hide file tree

Showing 29 changed files with 216 additions and 74 deletions.
diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java
@@ -366,7 +366,11 @@ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) {
    * @return The robot's pose
    */
   public Pose2d getPose() {
-    return swerveDrivePoseEstimator.getEstimatedPosition();
+
+    odometryLock.lock();
+    Pose2d poseEstimation = swerveDrivePoseEstimator.getEstimatedPosition();
+    odometryLock.unlock();
+    return poseEstimation;
   }
 
   /**
@@ -400,7 +404,9 @@ public ChassisSpeeds getRobotVelocity() {
    * @param pose The pose to set the odometry to
    */
   public void resetOdometry(Pose2d pose) {
+    odometryLock.lock();
     swerveDrivePoseEstimator.resetPosition(pose.getRotation(), getModulePositions(), pose);
+    odometryLock.unlock();
     kinematics.toSwerveModuleStates(
         ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, pose.getRotation()));
   }
@@ -569,7 +575,8 @@ public void setMaximumSpeed(double maximumSpeed, boolean updateModuleFeedforward
     //    swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond = maximumSpeed;
     swerveController.config.maxSpeed = maximumSpeed;
     //    swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond =
-    // swerveController.config.maxAngularVelocity;
+    // swerveController.config
+    //    .maxAngularVelocity;
     for (SwerveModule module : swerveModules) {
       module.configuration.maxSpeed = maximumSpeed;
       if (updateModuleFeedforward) {
@@ -649,58 +656,64 @@ public void replaceSwerveModuleFeedforward(SimpleMotorFeedforward feedforward) {
    */
   public void updateOdometry() {
     odometryLock.lock();
-    // Update odometry
-    swerveDrivePoseEstimator.update(getYaw(), getModulePositions());
+    try {
+      // Update odometry
+      swerveDrivePoseEstimator.update(getYaw(), getModulePositions());
 
-    // Update angle accumulator if the robot is simulated
-    if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) {
-      Pose2d[] modulePoses = getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition());
-      if (SwerveDriveTelemetry.isSimulation) {
-        simIMU.updateOdometry(kinematics, getStates(), modulePoses, field);
+      // Update angle accumulator if the robot is simulated
+      if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) {
+        Pose2d[] modulePoses =
+            getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition());
+        if (SwerveDriveTelemetry.isSimulation) {
+          simIMU.updateOdometry(kinematics, getStates(), modulePoses, field);
+        }
+
+        ChassisSpeeds measuredChassisSpeeds = getRobotVelocity();
+        SwerveDriveTelemetry.measuredChassisSpeeds[1] = measuredChassisSpeeds.vyMetersPerSecond;
+        SwerveDriveTelemetry.measuredChassisSpeeds[0] = measuredChassisSpeeds.vxMetersPerSecond;
+        SwerveDriveTelemetry.measuredChassisSpeeds[2] =
+            Math.toDegrees(measuredChassisSpeeds.omegaRadiansPerSecond);
+        SwerveDriveTelemetry.robotRotation = getYaw().getDegrees();
       }
 
-      ChassisSpeeds measuredChassisSpeeds = getRobotVelocity();
-      SwerveDriveTelemetry.measuredChassisSpeeds[1] = measuredChassisSpeeds.vyMetersPerSecond;
-      SwerveDriveTelemetry.measuredChassisSpeeds[0] = measuredChassisSpeeds.vxMetersPerSecond;
-      SwerveDriveTelemetry.measuredChassisSpeeds[2] =
-          Math.toDegrees(measuredChassisSpeeds.omegaRadiansPerSecond);
-      SwerveDriveTelemetry.robotRotation = getYaw().getDegrees();
-    }
+      if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) {
+        field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition());
+      }
 
-    if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) {
-      field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition());
-    }
+      double sumVelocity = 0;
+      for (SwerveModule module : swerveModules) {
+        SwerveModuleState moduleState = module.getState();
+        sumVelocity += Math.abs(moduleState.speedMetersPerSecond);
+        if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) {
+          SmartDashboard.putNumber(
+              "Module[" + module.configuration.name + "] Relative Encoder",
+              module.getRelativePosition());
+          SmartDashboard.putNumber(
+              "Module[" + module.configuration.name + "] Absolute Encoder",
+              module.getAbsolutePosition());
+        }
+        if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) {
+          SwerveDriveTelemetry.measuredStates[module.moduleNumber * 2] =
+              moduleState.angle.getDegrees();
+          SwerveDriveTelemetry.measuredStates[(module.moduleNumber * 2) + 1] =
+              moduleState.speedMetersPerSecond;
+        }
+      }
 
-    double sumVelocity = 0;
-    for (SwerveModule module : swerveModules) {
-      SwerveModuleState moduleState = module.getState();
-      sumVelocity += Math.abs(moduleState.speedMetersPerSecond);
-      if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) {
-        SmartDashboard.putNumber(
-            "Module[" + module.configuration.name + "] Relative Encoder",
-            module.getRelativePosition());
-        SmartDashboard.putNumber(
-            "Module[" + module.configuration.name + "] Absolute Encoder",
-            module.getAbsolutePosition());
+      // If the robot isn't moving synchronize the encoders every 100ms (Inspired by democrat's SDS
+      // lib)
+      // To ensure that everytime we initialize it works.
+      if (sumVelocity <= .01 && ++moduleSynchronizationCounter > 5) {
+        synchronizeModuleEncoders();
+        moduleSynchronizationCounter = 0;
       }
+
       if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) {
-        SwerveDriveTelemetry.measuredStates[module.moduleNumber * 2] =
-            moduleState.angle.getDegrees();
-        SwerveDriveTelemetry.measuredStates[(module.moduleNumber * 2) + 1] =
-            moduleState.speedMetersPerSecond;
+        SwerveDriveTelemetry.updateData();
       }
-    }
-
-    // If the robot isn't moving synchronize the encoders every 100ms (Inspired by democrat's SDS
-    // lib)
-    // To ensure that everytime we initialize it works.
-    if (sumVelocity <= .01 && ++moduleSynchronizationCounter > 5) {
-      synchronizeModuleEncoders();
-      moduleSynchronizationCounter = 0;
-    }
-
-    if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) {
-      SwerveDriveTelemetry.updateData();
+    } catch (Exception e) {
+      odometryLock.unlock();
+      throw e;
     }
     odometryLock.unlock();
   }
@@ -745,19 +758,22 @@ public void setGyroOffset(Rotation3d offset) {
    */
   public void addVisionMeasurement(
       Pose2d robotPose, double timestamp, boolean soft, double trustWorthiness) {
+    odometryLock.lock();
     if (soft) {
       swerveDrivePoseEstimator.addVisionMeasurement(
           robotPose, timestamp, visionMeasurementStdDevs.times(1.0 / trustWorthiness));
     } else {
       swerveDrivePoseEstimator.resetPosition(
           robotPose.getRotation(), getModulePositions(), robotPose);
     }
-
-    setGyroOffset(new Rotation3d(0, 0, robotPose.getRotation().getRadians()));
-    resetOdometry(
+    Pose2d newOdometry =
         new Pose2d(
             swerveDrivePoseEstimator.getEstimatedPosition().getTranslation(),
-            robotPose.getRotation()));
+            robotPose.getRotation());
+    odometryLock.unlock();
+
+    setGyroOffset(new Rotation3d(0, 0, robotPose.getRotation().getRadians()));
+    resetOdometry(newOdometry);
   }
 
   /**

diff --git a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java
@@ -0,0 +1,70 @@
+package swervelib.encoders;
+
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+
+/**
+ * DutyCycle encoders such as "US Digital MA3 with PWM Output, the CTRE Mag Encoder, the Rev Hex
+ * Encoder, and the AM Mag Encoder." attached via a PWM lane.
+ *
+ * <p>Credits to <a
+ * href="https://github.com/p2reneker25/2035-YAGSL/blob/main/swervelib/encoders/PWMDutyCycleEncoderSwerve.java">
+ * p2reneker25</a> for building this.
+ */
+public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder {
+
+  /** Duty Cycle Encoder. */
+  private final DutyCycleEncoder encoder;
+  /** Inversion state. */
+  private boolean isInverted;
+
+  /**
+   * Constructor for the PWM duty cycle encoder.
+   *
+   * @param pin PWM lane for the encoder.
+   */
+  public PWMDutyCycleEncoderSwerve(int pin) {
+    encoder = new DutyCycleEncoder(pin);
+  }
+
+  /**
+   * Configure the inversion state of the encoder.
+   *
+   * @param inverted Whether the encoder is inverted.
+   */
+  @Override
+  public void configure(boolean inverted) {
+    isInverted = inverted;
+  }
+
+  /**
+   * Get the absolute position of the encoder.
+   *
+   * @return Absolute position in degrees from [0, 360).
+   */
+  @Override
+  public double getAbsolutePosition() {
+    return (isInverted ? -1.0 : 1.0) * encoder.getAbsolutePosition() * 360;
+  }
+
+  /**
+   * Get the encoder object.
+   *
+   * @return {@link DutyCycleEncoder} from the class.
+   */
+  @Override
+  public Object getAbsoluteEncoder() {
+    return encoder;
+  }
+
+  /** Reset the encoder to factory defaults. */
+  @Override
+  public void factoryDefault() {
+    // Do nothing
+  }
+
+  /** Clear sticky faults on the encoder. */
+  @Override
+  public void clearStickyFaults() {
+    // Do nothing
+  }
+}
diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java
@@ -5,6 +5,8 @@
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.SerialPort;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import java.util.Optional;
@@ -31,7 +33,43 @@ public NavXSwerve(SerialPort.Port port) {
       factoryDefault();
       SmartDashboard.putData(gyro);
     } catch (RuntimeException ex) {
-      DriverStation.reportError("Error instantiating navX-MXP:  " + ex.getMessage(), true);
+      DriverStation.reportError("Error instantiating navX:  " + ex.getMessage(), true);
+    }
+  }
+
+  /**
+   * Constructor for the NavX swerve.
+   *
+   * @param port SPI Port to connect to.
+   */
+  public NavXSwerve(SPI.Port port) {
+    try {
+      /* Communicate w/navX-MXP via the MXP SPI Bus.                                     */
+      /* Alternatively:  I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB     */
+      /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
+      gyro = new AHRS(port);
+      factoryDefault();
+      SmartDashboard.putData(gyro);
+    } catch (RuntimeException ex) {
+      DriverStation.reportError("Error instantiating navX:  " + ex.getMessage(), true);
+    }
+  }
+
+  /**
+   * Constructor for the NavX swerve.
+   *
+   * @param port I2C Port to connect to.
+   */
+  public NavXSwerve(I2C.Port port) {
+    try {
+      /* Communicate w/navX-MXP via the MXP SPI Bus.                                     */
+      /* Alternatively:  I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB     */
+      /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
+      gyro = new AHRS(port);
+      factoryDefault();
+      SmartDashboard.putData(gyro);
+    } catch (RuntimeException ex) {
+      DriverStation.reportError("Error instantiating navX:  " + ex.getMessage(), true);
     }
   }
 

diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java
@@ -1,10 +1,14 @@
 package swervelib.parser.json;
 
 import com.revrobotics.SparkMaxRelativeEncoder.Type;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.SerialPort.Port;
 import swervelib.encoders.AnalogAbsoluteEncoderSwerve;
 import swervelib.encoders.CANCoderSwerve;
 import swervelib.encoders.CanAndCoderSwerve;
+import swervelib.encoders.PWMDutyCycleEncoderSwerve;
 import swervelib.encoders.SparkMaxEncoderSwerve;
 import swervelib.encoders.SwerveAbsoluteEncoder;
 import swervelib.imu.ADIS16448Swerve;
@@ -48,9 +52,14 @@ public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor) {
         return new SparkMaxEncoderSwerve(motor, 360);
       case "canandcoder_can":
         return new CanAndCoderSwerve(id);
+      case "ma3":
+      case "ctre_mag":
+      case "rev_hex":
+      case "am_mag":
+      case "dutycycle":
+        return new PWMDutyCycleEncoderSwerve(id);
       case "thrifty":
       case "throughbore":
-      case "dutycycle":
       case "analog":
         return new AnalogAbsoluteEncoderSwerve(id);
       case "cancoder":
@@ -75,6 +84,15 @@ public SwerveIMU createIMU() {
         return new ADXRS450Swerve();
       case "analog":
         return new AnalogGyroSwerve(id);
+      case "navx_spi":
+        return new NavXSwerve(SPI.Port.kMXP);
+      case "navx_i2c":
+        DriverStation.reportWarning(
+            "WARNING: There exists an I2C lockup issue on the roboRIO that could occur, more information here: "
+                + "\nhttps://docs.wpilib.org/en/stable/docs/yearly-overview/known-issues"
+                + ".html#onboard-i2c-causing-system-lockups",
+            false);
+        return new NavXSwerve(I2C.Port.kMXP);
       case "navx_onborard":
         return new NavXSwerve(Port.kOnboard);
       case "navx_usb":

diff --git a/yagsl/repos/swervelib/YAGSL-cpp/maven-metadata.xml b/yagsl/repos/swervelib/YAGSL-cpp/maven-metadata.xml
@@ -8,6 +8,6 @@
     <versions>
       <version>2023.0.6</version>
     </versions>
-    <lastUpdated>20230830025709</lastUpdated>
+    <lastUpdated>20230901052156</lastUpdated>
   </versioning>
 </metadata>
diff --git a/yagsl/repos/swervelib/YAGSL-cpp/maven-metadata.xml.md5 b/yagsl/repos/swervelib/YAGSL-cpp/maven-metadata.xml.md5
@@ -1 +1 @@
-af45abc8a3fd6fdf8b1d4a6ad5cd3953
+f208f00d818c034b803165d4761bfe02
diff --git a/yagsl/repos/swervelib/YAGSL-cpp/maven-metadata.xml.sha1 b/yagsl/repos/swervelib/YAGSL-cpp/maven-metadata.xml.sha1
@@ -1 +1 @@
-081b5a5bfc3916c09035bc05d4dbd15821e32131
+425936e72c607e24dbc7121b95cbfa8c8260f5a8
diff --git a/yagsl/repos/swervelib/YAGSL-cpp/maven-metadata.xml.sha256 b/yagsl/repos/swervelib/YAGSL-cpp/maven-metadata.xml.sha256
@@ -1 +1 @@
-5178d7973cd079217fbc37677ac1101f93ca5fcd9d9333e28076d8e9f9555668
+b454c20c98fb930296b84770d5839092a664facb2e898ecb58d3ce75f356e569
diff --git a/yagsl/repos/swervelib/YAGSL-cpp/maven-metadata.xml.sha512 b/yagsl/repos/swervelib/YAGSL-cpp/maven-metadata.xml.sha512
@@ -1 +1 @@
-1b28591f76f1042a8885b24e251227c55ea0a63fa135af00c48d006d7d1316ca53d4018c8bf72644749d2c5b86ec787d25aaed1f76ecc87da08b1a5fb120369e
+6b83b7c325dd3d2a6a72312d04980b053f81d1b33eab016c9e3d1710b52f7140e24fb2ab55a49815fc088f84db8a87d49d7ef9cb5d134af634b40c186ad0256d
diff --git a/yagsl/repos/swervelib/YAGSL-java/2023.0.6/YAGSL-java-2023.0.6-javadoc.jar b/yagsl/repos/swervelib/YAGSL-java/2023.0.6/YAGSL-java-2023.0.6-javadoc.jar
diff --git a/yagsl/repos/swervelib/YAGSL-java/2023.0.6/YAGSL-java-2023.0.6-javadoc.jar.md5 b/yagsl/repos/swervelib/YAGSL-java/2023.0.6/YAGSL-java-2023.0.6-javadoc.jar.md5
@@ -1 +1 @@
-c42d0787b015598645c01e7e67fdd2ed
+e19d633578d755be63610ef64147e6ff
diff --git a/yagsl/repos/swervelib/YAGSL-java/2023.0.6/YAGSL-java-2023.0.6-javadoc.jar.sha1 b/yagsl/repos/swervelib/YAGSL-java/2023.0.6/YAGSL-java-2023.0.6-javadoc.jar.sha1
@@ -1 +1 @@
-ae08ed1d5fe08c94b0416108a3c984eb122edc32
+f0ec5727e2e4d355b8cea889048762b7be07bc84
diff --git a/yagsl/repos/swervelib/YAGSL-java/2023.0.6/YAGSL-java-2023.0.6-javadoc.jar.sha256 b/yagsl/repos/swervelib/YAGSL-java/2023.0.6/YAGSL-java-2023.0.6-javadoc.jar.sha256
@@ -1 +1 @@
-99121689ad1b076d6cc126a6dfc92de66a692c03cbfad72af7034bf0ffb4a704
+26f7846f6c67eb9d358fd86a04dc353da252c3624783e3281f426d1c940a256e
diff --git a/yagsl/repos/swervelib/YAGSL-java/2023.0.6/YAGSL-java-2023.0.6-javadoc.jar.sha512 b/yagsl/repos/swervelib/YAGSL-java/2023.0.6/YAGSL-java-2023.0.6-javadoc.jar.sha512
@@ -1 +1 @@
-fd95e96124867d038ceaadfc8d4be1c1efac0e8fc3430babcd8898750786a734c4250b9467648d3f002390a3f959c61dd4c75ca04433848354596e81cb41afa3
+9dea91131c9d63ca895785c3e6f5e74542e990bc2d25fc0ddbfc0c9537f3822117bf14a49b87f87f95ce216866dd7be6e2a85a1d0fa6eaa4606ab23a96b442cc
diff --git a/yagsl/repos/swervelib/YAGSL-java/2023.0.6/YAGSL-java-2023.0.6-sources.jar b/yagsl/repos/swervelib/YAGSL-java/2023.0.6/YAGSL-java-2023.0.6-sources.jar
diff --git a/yagsl/repos/swervelib/YAGSL-java/2023.0.6/YAGSL-java-2023.0.6-sources.jar.md5 b/yagsl/repos/swervelib/YAGSL-java/2023.0.6/YAGSL-java-2023.0.6-sources.jar.md5
@@ -1 +1 @@
-d8d8b7f928c4df193ff1f2d418a16472
+e1142aa8a50409e62326d6b7ab8da93c
diff --git a/yagsl/repos/swervelib/YAGSL-java/2023.0.6/YAGSL-java-2023.0.6-sources.jar.sha1 b/yagsl/repos/swervelib/YAGSL-java/2023.0.6/YAGSL-java-2023.0.6-sources.jar.sha1
@@ -1 +1 @@
-b9255d677764b8c7da30b2ae334b1caf804a341c
+df195a1939eeeedc576d777da85a4da1b05a418d