Merge pull request #105 from Technologyman00/main

Add Setting the Absolute Encoder Offset. Amazing changes to fix the known "flailing" issue.

README.md

@@ -105,4 +105,8 @@ SwerveDrive swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumS
 
 * Implement momentum velocity limitations in SwerveMath.
 
-### Ensure the IMU is centered on the robot
+### Ensure the IMU is centered on the robot
+
+# Maintainers
+- @thenetworkgrinch
+- @Technologyman00

src/main/java/swervelib/SwerveDrive.java

@@ -1071,4 +1071,27 @@ public void setModuleStateOptimization(boolean optimizationEnabled)
     }
   }
 
+  /**
+   * Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type.
+   * Also removes the internal offsets to prevent double offsetting.
+   */
+  public void pushOffsetsToControllers()
+  {
+    for (SwerveModule module : swerveModules)
+    {
+      module.pushOffsetsToControllers();
+    }
+  }
+
+  /**
+   * Restores Internal YAGSL Encoder offsets and sets the Encoder stored offset back to 0
+   */
+  public void restoreInternalOffset()
+  {
+    for (SwerveModule module : swerveModules)
+    {
+      module.restoreInternalOffset();
+    }
+  }
+
 }

src/main/java/swervelib/SwerveModule.java

@@ -4,6 +4,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import swervelib.encoders.SwerveAbsoluteEncoder;
 import swervelib.math.SwerveMath;
@@ -26,7 +27,7 @@ public class SwerveModule
   /**
    * Angle offset from the absolute encoder.
    */
-  private final double                    angleOffset;
+  private double                    angleOffset;
   /**
    * Swerve Motors.
    */
@@ -388,6 +389,34 @@ public SwerveModuleConfiguration getConfiguration()
     return configuration;
   }
 
+  /**
+   * Push absolute encoder offset in the memory of the encoder or controller.
+   * Also removes the internal angle offset.
+   */
+  public void pushOffsetsToControllers()
+  {
+    if(absoluteEncoder != null)
+    {
+      if(absoluteEncoder.setAbsoluteEncoderOffset(angleOffset)){
+        angleOffset = 0;
+      }
+      else{
+        DriverStation.reportWarning("Pushing the Absolute Encoder offset to the encoder failed on module #"+moduleNumber, false);
+      }
+    }
+    else{
+      DriverStation.reportWarning("There is no Absolute Encoder on module #"+moduleNumber, false);
+    }
+  }
+
+  /**
+   * Restore internal offset in YAGSL and either sets absolute encoder offset to 0 or restores old value.
+   */
+  public void restoreInternalOffset()
+  {
+    absoluteEncoder.setAbsoluteEncoderOffset(0);
+    angleOffset = configuration.angleOffset;
+  }
   /**
    * Get if the last Absolute Encoder had a read issue, such as it does not exist.
    *

src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java

@@ -3,6 +3,7 @@
 import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.DriverStation;
 
 /**
  * Swerve Absolute Encoder for Thrifty Encoders and other analog encoders.
@@ -91,6 +92,21 @@ public Object getAbsoluteEncoder()
     return encoder;
   }
 
+  /**
+   * Cannot Set the offset of an Analog Absolute Encoder.
+   * 
+   * @param offset the offset the Absolute Encoder uses as the zero point.
+   * 
+   * @return Will always be false as setting the offset is unsupported of an Analog absolute encoder.
+   */
+  @Override
+  public boolean setAbsoluteEncoderOffset(double offset)
+  {
+    //Do Nothing
+    DriverStation.reportWarning("Cannot Set Absolute Encoder Offset of Analog Encoders Channel #"+encoder.getChannel(), false);
+    return false;
+   }
+
   /**
    * Get the velocity in degrees/sec.
    *

src/main/java/swervelib/encoders/CANCoderSwerve.java

@@ -138,6 +138,25 @@ public Object getAbsoluteEncoder()
     return encoder;
   }
 
+  /**
+   * Sets the Absolute Encoder Offset inside of the CANcoder's Memory.
+   * 
+   * @param offset the offset the Absolute Encoder uses as the zero point.
+   * 
+   * @return if setting Absolute Encoder Offset was successful or not.
+   */
+  @Override
+  public boolean setAbsoluteEncoderOffset(double offset)
+  {
+    ErrorCode error = encoder.configMagnetOffset(offset);
+    if(error == ErrorCode.OK)
+    {
+      return true;
+    }
+    DriverStation.reportWarning("Failure to set CANCoder " + encoder.getDeviceID() + " Absolute Encoder Offset Error: "+error, false);
+    return false;
+   }
+
   /**
    * Get the velocity in degrees/sec.
    *

src/main/java/swervelib/encoders/CanAndCoderSwerve.java

@@ -2,6 +2,8 @@
 
 import com.reduxrobotics.sensors.canandcoder.CANandcoder;
 
+import edu.wpi.first.wpilibj.DriverStation;
+
 /**
  * HELIUM {@link CANandcoder} from ReduxRobotics absolute encoder, attached through the CAN bus.
  */
@@ -78,6 +80,21 @@ public Object getAbsoluteEncoder()
     return encoder;
   }
 
+  /**
+   * Cannot set the offset of the CanAndCoder.
+   * 
+   * @param offset the offset the Absolute Encoder uses as the zero point.
+   * 
+   * @return always false due to CanAndCoder not supporting offset changing.
+   */
+  @Override
+  public boolean setAbsoluteEncoderOffset(double offset)
+  {
+    //CanAndCoder does not support Absolute Offset Changing
+    DriverStation.reportWarning("Cannot Set Absolute Encoder Offset of CanAndCoders ID: "+encoder.getAddress(), false);
+    return false;
+  }
+
   /**
    * Get the velocity in degrees/sec.
    *

src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java

@@ -96,4 +96,18 @@ public void clearStickyFaults()
     // Do nothing
   }
 
+  /**
+   * Sets the offset of the Encoder in the WPILib Encoder Library.
+   * 
+   * @param offset the offset the Absolute Encoder uses as the zero point.
+   * 
+   * @return Always true due to no external device commands.
+   */
+  @Override
+  public boolean setAbsoluteEncoderOffset(double offset)
+  {
+    encoder.setPositionOffset(offset);
+
+    return true;
+  }
 }

src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java

@@ -107,6 +107,28 @@ public Object getAbsoluteEncoder()
   }
 
   /**
+   * Sets the Absolute Encoder Offset inside of the SparkMax's Memory.
+   * 
+   * @param offset the offset the Absolute Encoder uses as the zero point.
+   * 
+   * @return if setting Absolute Encoder Offset was successful or not.
+   */
+  @Override
+  public boolean setAbsoluteEncoderOffset(double offset)
+  {
+    REVLibError error = null;
+    for(int i=0; i < maximumRetries; i++)
+    {
+      error = encoder.setZeroOffset(offset);
+      if(error == REVLibError.kOk)
+      {
+        return true;
+      }
+    }
+    DriverStation.reportWarning("Failure to set Absolute Encoder Offset Error: "+ error, false);
+    return false;
+  }
+  /** 
    * Get the velocity in degrees/sec.
    *
    * @return velocity in degrees/sec.

src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java

@@ -47,6 +47,14 @@ public abstract class SwerveAbsoluteEncoder
   public abstract Object getAbsoluteEncoder();
 
   /**
+   * Sets the Absolute Encoder offset at the Encoder Level.
+   * 
+   * @param offset the offset the Absolute Encoder uses as the zero point.
+   * 
+   * @return if setting Absolute Encoder Offset was successful or not.
+   */
+  public abstract boolean setAbsoluteEncoderOffset(double offset);
+  /*
    * Get the velocity in degrees/sec.
    * @return velocity in degrees/sec.
    */