Fixes

FRC-7525 · Nov 14, 2024 · b59243b · b59243b
1 parent a5673f8
commit b59243b
Show file tree

Hide file tree

Showing 9 changed files with 44 additions and 28 deletions.
diff --git a/...n/java/frc/robot/pioneersLib/bumSwerve/SwerveAbsoluteEncoder/SwerveAbsoluteEncoderIO.java b/...n/java/frc/robot/pioneersLib/bumSwerve/SwerveAbsoluteEncoder/SwerveAbsoluteEncoderIO.java
@@ -27,7 +27,7 @@ public static class SwerveAbsoluteEncoderIOInputs {
     public Rotation2d getTurnAbsolutePosition();
 
     /*Returns position status signal */
-    public double getRotationDeg();
+    // public double getRotationDeg();
 
     public boolean isSim();
 

diff --git a/...rc/robot/pioneersLib/bumSwerve/SwerveAbsoluteEncoder/SwerveAbsoluteEncoderIOCANcoder.java b/...rc/robot/pioneersLib/bumSwerve/SwerveAbsoluteEncoder/SwerveAbsoluteEncoderIOCANcoder.java
@@ -10,6 +10,7 @@
 import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.CAN;
@@ -51,7 +52,7 @@ public void updateInputs(SwerveAbsoluteEncoderIOInputs inputs) {
 
         inputs.absoluteEncoderOffset = absoluteEncoderOffset;
         inputs.inverted = inverted;
-        inputs.turnAbsolutePosition = (CaNcoder.getAbsolutePosition().getValueAsDouble() * 360) + absoluteEncoderOffset;
+        inputs.turnAbsolutePosition = MathUtil.angleModulus(Units.degreesToRadians(CaNcoder.getAbsolutePosition().getValueAsDouble() * 360 + absoluteEncoderOffset));
     }
 
     @Override
@@ -66,6 +67,7 @@ public void setInverted(boolean inverted) {
 
     @Override
     public Rotation2d getTurnAbsolutePosition() {
+        // CHECK: Subtracting offset instead of adding offset
         return Rotation2d.fromRotations(CaNcoder.getAbsolutePosition().getValueAsDouble()).plus(Rotation2d.fromDegrees(absoluteEncoderOffset));
         // return new Rotation2d();
     }
@@ -75,10 +77,10 @@ public void setRotationDeg(double rotationDeg) {
         return;
     }
 
-    @Override
-    public double getRotationDeg() {
-        return getTurnAbsolutePosition().getDegrees();
-    }
+    // @Override
+    // public double getRotationDeg() {
+    //     // return getTurnAbsolutePosition().getDegrees();
+    // }
 
     @Override
     public boolean isSim() {

diff --git a/...ava/frc/robot/pioneersLib/bumSwerve/SwerveAbsoluteEncoder/SwerveAbsoluteEncoderIOSim.java b/...ava/frc/robot/pioneersLib/bumSwerve/SwerveAbsoluteEncoder/SwerveAbsoluteEncoderIOSim.java
@@ -64,10 +64,10 @@ public void setRotationDeg(double rotationDeg) {
         CANcoderController.setRawPosition(rotationDeg / 360);
     }
 
-    @Override
-    public double getRotationDeg() {
-        return turnAbsolutePosition.getValueAsDouble() / 360;
-    }
+    // @Override
+    // public double getRotationDeg() {
+    //     return turnAbsolutePosition.getValueAsDouble() / 360;
+    // }
 
     @Override 
     public boolean isSim() {

diff --git a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveModule.java b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveModule.java
@@ -2,7 +2,8 @@
 
 import org.littletonrobotics.junction.Logger;
 
-import edu.wpi.first.math.MathUtil;
+import com.ctre.phoenix6.StatusCode;
+
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -20,7 +21,7 @@ public class SwerveModule {
     private SwerveMotorIO driveMotor;
     private SwerveMotorIO turnMotor;
     private SwerveAbsoluteEncoderIO absoluteEncoder;
-    private Rotation2d turnRelativeEncoderOffset;
+    // private Rotation2d turnRelativeEncoderOffset;
 
     private SwerveModulePosition[] odometryPositions;
 
@@ -33,6 +34,8 @@ public class SwerveModule {
     private Double speedSetPoint;
     private Double angleSetPoint;
 
+    private boolean offsetedTurnEncoder = false;
+
     private SwerveModuleState lastModuleState;
 
     // Starting threshold for anti-jitter
@@ -107,10 +110,14 @@ public void antiJitter(
      */
     public SwerveModuleState runState(SwerveModuleState state) {
         // Finds encoder offset that's used for odo calculations
-        if (turnRelativeEncoderOffset == null) {
-            turnMotor.setEncoderPosition(absoluteEncoder.getTurnAbsolutePosition().getDegrees());
-            // turnRelativeEncoderOffset =  absoluteEncoder.getTurnAbsolutePosition().minus(turnMotor.getAngle());
-            turnRelativeEncoderOffset = new Rotation2d();
+        // if (turnRelativeEncoderOffset == null) {
+        //     turnMotor.setEncoderPosition(absoluteEncoder.getTurnAbsolutePosition().getDegrees());
+        //     // turnRelativeEncoderOffset =  absoluteEncoder.getTurnAbsolutePosition().minus(turnMotor.getAngle());
+        //     turnRelativeEncoderOffset = new Rotation2d();
+        // }
+
+        if(!offsetedTurnEncoder && absoluteEncoder.getTurnAbsolutePosition().getDegrees() != 0) {
+            offsetedTurnEncoder = turnMotor.setEncoderPosition(absoluteEncoder.getTurnAbsolutePosition().getDegrees()) == StatusCode.OK ?  true : false;
         }
 
         //feeds value directly to encoder if it is sim.
@@ -155,12 +162,10 @@ public void periodic() {
 		for (int i = 0; i < sampleCount; i++) {
 			double positionMeters = driveInputs.odometryDriveAccumulatedPosition[i] * 2 * Math.PI * SwerveDrive.wheelRadius;
 			Rotation2d angle =
-				turnInputs.odometryMotorPositions[i].plus(
-						turnRelativeEncoderOffset != null ? turnRelativeEncoderOffset : new Rotation2d()
-					);
+				turnInputs.odometryMotorPositions[i];
 			odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
 		}
-    }
+    } 
 
     public void updateInputs() {
         turnMotor.updateInputs(turnInputs);

diff --git a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIO.java b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIO.java
@@ -2,6 +2,8 @@
 
 import org.littletonrobotics.junction.AutoLog;
 
+import com.ctre.phoenix6.StatusCode;
+
 import edu.wpi.first.math.geometry.Rotation2d;
 
 public interface SwerveMotorIO {
@@ -53,7 +55,9 @@ public default double getVelocity() {
     /**
      * Sets the motors position
      */
-    public default void setEncoderPosition(double positionDeg) {}
+    public default StatusCode setEncoderPosition(double positionDeg) {
+        return StatusCode.OK;
+    }
 
     /**
      * @return The current position error of the motor from the feedback controller (in rotations)

diff --git a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIONeoSim.java b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIONeoSim.java
@@ -1,5 +1,6 @@
 package frc.robot.pioneersLib.bumSwerve.SwerveMotor;
 
+import com.ctre.phoenix6.StatusCode;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.REVLibError;
 import com.revrobotics.REVPhysicsSim;
@@ -110,8 +111,9 @@ public void setVoltage(double volts) {
     }   
 
     @Override
-    public void setEncoderPosition(double positionDeg) {
+    public StatusCode setEncoderPosition(double positionDeg) {
         encoder.setPosition(positionDeg / 360);
+        return StatusCode.OK;
     }
 
     @Override

diff --git a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOSparkMax.java b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOSparkMax.java
@@ -3,6 +3,7 @@
 import java.util.OptionalDouble;
 import java.util.Queue;
 
+import com.ctre.phoenix6.StatusCode;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.REVLibError;
 import com.revrobotics.RelativeEncoder;
@@ -145,8 +146,9 @@ public void setPosition(double setpoint) {
     }
 
     @Override
-    public void setEncoderPosition(double positionDeg) {
+    public StatusCode setEncoderPosition(double positionDeg) {
         encoder.setPosition(positionDeg / 360);
+        return StatusCode.OK;
     }
 
     @Override

diff --git a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFX.java b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFX.java
@@ -3,6 +3,7 @@
 import java.util.Queue;
 
 import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.FeedbackConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
@@ -143,9 +144,8 @@ public double getVelocity() {
     }
 
     @Override
-    public void setEncoderPosition(double positionDeg) {
-        System.out.println(positionDeg);
-        motor.setPosition(Units.degreesToRotations(positionDeg) * gearRatio);
+    public StatusCode setEncoderPosition(double positionDeg) {
+        return motor.setPosition(Units.degreesToRotations(positionDeg) * gearRatio);
         // motor.setPosition((positionDeg/360) * gearRatio);
     }
 

diff --git a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFXSim.java b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFXSim.java
@@ -3,6 +3,7 @@
 import java.util.Queue;
 
 import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.FeedbackConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
@@ -123,8 +124,8 @@ public void setVoltage(double volts) {
     }
 
     @Override
-    public void setEncoderPosition(double positionDeg) {
-        dummyTalon.setPosition(positionDeg/360);
+    public StatusCode setEncoderPosition(double positionDeg) {
+        return dummyTalon.setPosition((positionDeg/360) * gearing);
     }
 
     @Override