diff --git a/README.md b/README.md index d4301eecc..05f5d85e1 100644 --- a/README.md +++ b/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 \ No newline at end of file +### Ensure the IMU is centered on the robot + +# Maintainers +- @thenetworkgrinch +- @Technologyman00 \ No newline at end of file diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 80764cffa..f270e2c6f 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/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(); + } + } + } diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index e1478e80c..bcc9a39a9 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/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. * diff --git a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java index d46f1d169..600e137b5 100644 --- a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java +++ b/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. * diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java index 5731dbe31..4c7637de5 100644 --- a/src/main/java/swervelib/encoders/CANCoderSwerve.java +++ b/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. * diff --git a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java b/src/main/java/swervelib/encoders/CanAndCoderSwerve.java index 639a2bd32..64ea4719d 100644 --- a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java +++ b/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. * diff --git a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java index 85c78bf1d..e85ba8164 100644 --- a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java +++ b/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; + } } \ No newline at end of file diff --git a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java index fd0d0bf69..cc178adea 100644 --- a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/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. diff --git a/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java b/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java index db0b7eb69..387c6e91d 100644 --- a/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java +++ b/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. */