Code cleanup

lasarobotics · Sep 6, 2024 · 1125b53 · 1125b53
1 parent 5df0d96
commit 1125b53
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 13 deletions.
diff --git a/src/main/java/org/lasarobotics/drive/TractionControlController.java b/src/main/java/org/lasarobotics/drive/TractionControlController.java
@@ -41,7 +41,7 @@ private State(int value) {
   private final double MAX_SLIP_RATIO = 0.40;
   private final int SIGMOID_K = 10;
   private final Measure<Velocity<Distance>> INERTIAL_VELOCITY_THRESHOLD = Units.MetersPerSecond.of(0.01);
-  private final Measure<Time> MIN_SLIPPING_TIME = Units.Seconds.of(1.1);
+  private final Measure<Time> MIN_SLIPPING_TIME = Units.Seconds.of(0.9);
 
   private double m_optimalSlipRatio;
   private double m_mass;
@@ -118,19 +118,14 @@ public Measure<Velocity<Distance>> calculate(Measure<Velocity<Distance>> velocit
       & isEnabled()
     );
 
-    // Get desired acceleration, capping at max possible acceleration
-    // double requestedAcceleration = velocityRequest.minus(inertialVelocity).per(Units.Seconds.of(GlobalConstants.ROBOT_LOOP_PERIOD)).in(Units.MetersPerSecondPerSecond);
-    // var desiredAcceleration = Units.MetersPerSecond.of(Math.copySign(
-    //   Math.min(Math.abs(requestedAcceleration), m_maxAcceleration / GlobalConstants.ROBOT_LOOP_HZ),
-    //   requestedAcceleration
-    // )).per(Units.Seconds.of(GlobalConstants.ROBOT_LOOP_PERIOD));
+    // Get desired acceleration
     var desiredAcceleration = velocityRequest.minus(inertialVelocity).per(Units.Seconds.of(GlobalConstants.ROBOT_LOOP_PERIOD));
 
     // Get sigmoid value
     double sigmoid = 1 / (1 + Math.exp(-SIGMOID_K * MathUtil.clamp(2 * (currentSlipRatio - m_optimalSlipRatio) - 1, -1.0, +1.0)));
 
     // Scale CoF based on whether or not robot is currently slipping
-    double effectiveCoF =  m_isSlipping ? m_staticCoF * (1 - sigmoid) + m_dynamicCoF * sigmoid : m_staticCoF;
+    double effectiveCoF = m_isSlipping ? m_staticCoF * (1 - sigmoid) + m_dynamicCoF * sigmoid : m_staticCoF;
 
     // Simplified prediction of future slip ratio based on desired acceleration
     double predictedSlipRatio = Math.abs(
@@ -139,7 +134,7 @@ & isEnabled()
           + effectiveCoF * m_mass * GlobalConstants.GRAVITATIONAL_ACCELERATION.in(Units.MetersPerSecondPerSecond))
     ) / m_maxPredictedSlipRatio;
 
-    // Calculate correction based on difference between optimal and weighted slip ratio, which combines the predicted and current slip ratios
+    // Calculate correction based on difference between optimal and predicted slip ratio, which combines the predicted and current slip ratios
     var velocityCorrection = velocityOutput.times((m_optimalSlipRatio - predictedSlipRatio) * m_state.value);
 
     // Update output, clamping to max linear speed

diff --git a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java
@@ -918,13 +918,14 @@ public void set(double value, ControlType ctrl, double arbFeedforward, SparkPIDC
   /**
    * Set the conversion factor for position of the sensor. Multiplied by the native output units to
    * give you position.
+   * <p>
+   * Passing in {@link FeedbackSensor#ABSOLUTE_ENCODER} or {@link FeedbackSensor#FUSED_ENCODER}
+   * will set the conversion factor for a connected absolute encoder.
    * @param sensor Sensor to set conversion factor for
    * @param factor The conversion factor to multiply the native units by
    * @return {@link REVLibError#kOk} if successful
    */
   public REVLibError setPositionConversionFactor(FeedbackSensor sensor, double factor) {
-    if (sensor.equals(FeedbackSensor.FUSED_ENCODER))
-      throw new IllegalArgumentException("Conversion factors must be set for motor encoder and absolute encoder separately when using fused mode!");
     REVLibError status;
     Supplier<REVLibError> parameterSetter;
     BooleanSupplier parameterCheckSupplier;
@@ -937,6 +938,7 @@ public REVLibError setPositionConversionFactor(FeedbackSensor sensor, double fac
         parameterSetter = () -> getAnalog().setPositionConversionFactor(factor);
         parameterCheckSupplier = () -> Precision.equals(getAnalog().getPositionConversionFactor(), factor, EPSILON);
         break;
+      case FUSED_ENCODER:
       case ABSOLUTE_ENCODER:
         parameterSetter = () -> getAbsoluteEncoder().setPositionConversionFactor(factor);
         parameterCheckSupplier = () -> Precision.equals(getAbsoluteEncoder().getPositionConversionFactor(), factor, EPSILON);
@@ -975,13 +977,14 @@ public double getPositionConversionFactor(FeedbackSensor sensor) {
   /**
    * Set the conversion factor for velocity of the sensor. Multiplied by the native output units to
    * give you velocity.
+   * <p>
+   * Passing in {@link FeedbackSensor#ABSOLUTE_ENCODER} or {@link FeedbackSensor#FUSED_ENCODER}
+   * will set the conversion factor for a connected absolute encoder.
    * @param sensor Sensor to set conversion factor for
    * @param factor The conversion factor to multiply the native units by
    * @return {@link REVLibError#kOk} if successful
    */
   public REVLibError setVelocityConversionFactor(FeedbackSensor sensor, double factor) {
-    if (sensor.equals(FeedbackSensor.FUSED_ENCODER))
-      throw new IllegalArgumentException("Conversion factors must be set for motor encoder and absolute encoder separately when using fused mode!");
     REVLibError status;
     Supplier<REVLibError> parameterSetter;
     BooleanSupplier parameterCheckSupplier;
@@ -994,6 +997,7 @@ public REVLibError setVelocityConversionFactor(FeedbackSensor sensor, double fac
         parameterSetter = () -> getAnalog().setVelocityConversionFactor(factor);
         parameterCheckSupplier = () -> Precision.equals(getAnalog().getVelocityConversionFactor(), factor, EPSILON);
         break;
+      case FUSED_ENCODER:
       case ABSOLUTE_ENCODER:
         parameterSetter = () -> getAbsoluteEncoder().setVelocityConversionFactor(factor);
         parameterCheckSupplier = () -> Precision.equals(getAnalog().getVelocityConversionFactor(), factor, EPSILON);
@@ -1158,6 +1162,8 @@ public void smoothMotion(double value, TrapezoidProfile.Constraints motionConstr
    * <p>
    * Sets the NEO encoder conversion factor to the absolute encoder conversion factor
    * divided by the specified ratio.
+   * Call {@link Spark#setPositionConversionFactor(FeedbackSensor, double)} and/or {@link Spark#setVelocityConversionFactor(FeedbackSensor, double)}
+   * <i>before</i> this method.
    * <p>
    * {@link Spark#setPositionConversionFactor(FeedbackSensor, double)} and {@link Spark#setVelocityConversionFactor(FeedbackSensor, double)}
    * should be called for the absolute encoder before this.