Adding Support for PWM Absolute Mag Encoders

src/main/java/frc/lib/util/AbsoluteEncoder.java

@@ -0,0 +1,42 @@
+package frc.lib.util;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.Constants;
+
+public class AbsoluteEncoder {
+
+    private final AnalogInput analogInput;
+
+    private Rotation2d rotation2d = new Rotation2d();
+    private Rotation2d home = new Rotation2d();
+
+    public AbsoluteEncoder(int port) {
+
+        analogInput = new AnalogInput(port);
+
+
+    }
+
+    public void homeEncoder() {
+
+        this.home = this.rotation2d;
+
+    }
+
+    public double getAbsolutePosition() {
+        double voltage = analogInput.getVoltage();
+        double degrees = 360 * (voltage / Constants.MAX_ENCODER_VOLTAGE);
+        return degrees;
+    }
+
+    /*
+    public void getRotationPeriodic() {
+
+        this.rotation2d = Rotation2d.fromDegrees(360 * (analogInput.getVoltage() / 5.0));
+        System.out.println(rotation2d);
+
+    }
+    */
+}

src/main/java/frc/lib/util/SwerveAbsoluteEncoder.java

@@ -0,0 +1,34 @@
+package frc.lib.util;
+
+public abstract class SwerveAbsoluteEncoder {
+  /**
+   * Reset the encoder to factory defaults.
+   */
+  public abstract void factoryDefault();
+
+  /**
+   * Clear sticky faults on the encoder.
+   */
+  public abstract void clearStickyFaults();
+
+  /**
+   * Configure the absolute encoder to read from [0, 360) per second.
+   *
+   * @param inverted Whether the encoder is inverted.
+   */
+  public abstract void configure(boolean inverted);
+
+  /**
+   * Get the absolute position of the encoder.
+   *
+   * @return Absolute position in degrees from [0, 360).
+   */
+  public abstract double getAbsolutePosition();
+
+  /**
+   * Get the instantiated absolute encoder Object.
+   *
+   * @return Absolute encoder object.
+   */
+  public abstract Object getAbsoluteEncoder();
+}

src/main/java/frc/lib/util/SwerveModuleConstants.java

@@ -5,20 +5,20 @@
 public class SwerveModuleConstants {
     public final int driveMotorID;
     public final int angleMotorID;
-    public final int cancoderID;
+    public final int encoderID;
     public final Rotation2d angleOffset;
 
     /**
      * Swerve Module Constants to be used when creating swerve modules.
      * @param driveMotorID
      * @param angleMotorID
-     * @param canCoderID
+     * @param encoderID
      * @param angleOffset
      */
-    public SwerveModuleConstants(int driveMotorID, int angleMotorID, int canCoderID, Rotation2d angleOffset) {
+    public SwerveModuleConstants(int driveMotorID, int angleMotorID, int encoderID, Rotation2d angleOffset) {
         this.driveMotorID = driveMotorID;
         this.angleMotorID = angleMotorID;
-        this.cancoderID = canCoderID;
+        this.encoderID = encoderID;
         this.angleOffset = angleOffset;
     }
 }

src/main/java/frc/robot/Constants.java

@@ -1,6 +1,7 @@
 package frc.robot;
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.sensors.CANCoder;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -12,11 +13,16 @@
 
 public final class Constants {
     public static final double stickDeadband = 0.1;
+    public static final double MAX_ENCODER_VOLTAGE = 4.926;
 
     public static final class Swerve {
+        public enum EncoderType { CANCODER, PWM }
+
         public static final int pigeonID = 1;
         public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW-
 
+        public static final EncoderType encoderType = EncoderType.CANCODER;
+
         public static final COTSFalconSwerveConstants chosenModule =  //TODO: This must be tuned to specific robot
             COTSFalconSwerveConstants.SDSMK4i(COTSFalconSwerveConstants.driveGearRatios.SDSMK4i_L2);
 
@@ -93,40 +99,40 @@ public static final class Swerve {
         public static final class Mod0 { //TODO: This must be tuned to specific robot
             public static final int driveMotorID = 1;
             public static final int angleMotorID = 2;
-            public static final int canCoderID = 1;
+            public static final int encoderID = 1;
             public static final Rotation2d angleOffset = Rotation2d.fromDegrees(0.0);
             public static final SwerveModuleConstants constants = 
-                new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset);
+                new SwerveModuleConstants(driveMotorID, angleMotorID, encoderID, angleOffset);
         }
 
         /* Front Right Module - Module 1 */
         public static final class Mod1 { //TODO: This must be tuned to specific robot
             public static final int driveMotorID = 3;
             public static final int angleMotorID = 4;
-            public static final int canCoderID = 2;
+            public static final int encoderID = 2;
             public static final Rotation2d angleOffset = Rotation2d.fromDegrees(0.0);
             public static final SwerveModuleConstants constants = 
-                new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset);
+                new SwerveModuleConstants(driveMotorID, angleMotorID, encoderID, angleOffset);
         }
 
         /* Back Left Module - Module 2 */
         public static final class Mod2 { //TODO: This must be tuned to specific robot
             public static final int driveMotorID = 5;
             public static final int angleMotorID = 6;
-            public static final int canCoderID = 3;
+            public static final int encoderID = 3;
             public static final Rotation2d angleOffset = Rotation2d.fromDegrees(0.0);
             public static final SwerveModuleConstants constants = 
-                new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset);
+                new SwerveModuleConstants(driveMotorID, angleMotorID, encoderID, angleOffset);
         }
 
         /* Back Right Module - Module 3 */
         public static final class Mod3 { //TODO: This must be tuned to specific robot
             public static final int driveMotorID = 7;
             public static final int angleMotorID = 8;
-            public static final int canCoderID = 4;
+            public static final int encoderID = 4;
             public static final Rotation2d angleOffset = Rotation2d.fromDegrees(0.0);
             public static final SwerveModuleConstants constants = 
-                new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset);
+                new SwerveModuleConstants(driveMotorID, angleMotorID, encoderID, angleOffset);
         }
     }
 

src/main/java/frc/robot/SwerveModule.java

@@ -6,6 +6,7 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 
 import frc.lib.math.Conversions;
+import frc.lib.util.AbsoluteEncoder;
 import frc.lib.util.CTREModuleState;
 import frc.lib.util.SwerveModuleConstants;
 
@@ -14,14 +15,14 @@
 import com.ctre.phoenix.motorcontrol.can.TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
 
-public class SwerveModule {
+public class SwerveModule<T> {
     public int moduleNumber;
     private Rotation2d angleOffset;
     private Rotation2d lastAngle;
 
     private TalonFX mAngleMotor;
     private TalonFX mDriveMotor;
-    private CANCoder angleEncoder;
+    private T angleEncoder;
 
     SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(Constants.Swerve.driveKS, Constants.Swerve.driveKV, Constants.Swerve.driveKA);
 
@@ -33,6 +34,9 @@ public SwerveModule(int moduleNumber, SwerveModuleConstants moduleConstants){
         angleEncoder = new CANCoder(moduleConstants.cancoderID);
         configAngleEncoder();
 
+        // absoluteEncoder = new AbsoluteEncoder(moduleConstants.encoderID);
+        // configAngleEncoder();
+
         /* Angle Motor Config */
         mAngleMotor = new TalonFX(moduleConstants.angleMotorID);
         configAngleMotor();
@@ -64,7 +68,6 @@ private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop){
 
     private void setAngle(SwerveModuleState desiredState){
         Rotation2d angle = (Math.abs(desiredState.speedMetersPerSecond) <= (Constants.Swerve.maxSpeed * 0.01)) ? lastAngle : desiredState.angle; //Prevent rotating module if speed is less then 1%. Prevents Jittering.
-
         mAngleMotor.set(ControlMode.Position, Conversions.degreesToFalcon(angle.getDegrees(), Constants.Swerve.angleGearRatio));
         lastAngle = angle;
     }
@@ -73,18 +76,22 @@ private Rotation2d getAngle(){
         return Rotation2d.fromDegrees(Conversions.falconToDegrees(mAngleMotor.getSelectedSensorPosition(), Constants.Swerve.angleGearRatio));
     }
 
-    public Rotation2d getCanCoder(){
-        return Rotation2d.fromDegrees(angleEncoder.getAbsolutePosition());
+    public Rotation2d getEncoderAngle(){
+
+        //return Rotation2d.fromDegrees(angleEncoder.getAbsolutePosition());
+
+        return Rotation2d.fromDegrees(absoluteEncoder.getAbsolutePosition());
     }
 
+
     public void resetToAbsolute(){
-        double absolutePosition = Conversions.degreesToFalcon(getCanCoder().getDegrees() - angleOffset.getDegrees(), Constants.Swerve.angleGearRatio);
+        double absolutePosition = Conversions.degreesToFalcon(absoluteEncoder.getRotation().getDegrees() - angleOffset.getDegrees(), Constants.Swerve.angleGearRatio); 
         mAngleMotor.setSelectedSensorPosition(absolutePosition);
     }
 
     private void configAngleEncoder(){        
-        angleEncoder.configFactoryDefault();
-        angleEncoder.configAllSettings(Robot.ctreConfigs.swerveCanCoderConfig);
+        // angleEncoder.configFactoryDefault();
+        // angleEncoder.configAllSettings(Robot.ctreConfigs.swerveCanCoderConfig);
     }
 
     private void configAngleMotor(){
@@ -103,6 +110,8 @@ private void configDriveMotor(){
         mDriveMotor.setSelectedSensorPosition(0);
     }
 
+    private 
+
     public SwerveModuleState getState(){
         return new SwerveModuleState(
             Conversions.falconToMPS(mDriveMotor.getSelectedSensorVelocity(), Constants.Swerve.wheelCircumference, Constants.Swerve.driveGearRatio), 

src/main/java/frc/robot/commands/TeleopSwerve.java

@@ -7,34 +7,102 @@
 import java.util.function.DoubleSupplier;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 
 
 public class TeleopSwerve extends CommandBase {    
-    private Swerve s_Swerve;    
+    public Swerve s_Swerve;    
     private DoubleSupplier translationSup;
     private DoubleSupplier strafeSup;
     private DoubleSupplier rotationSup;
     private BooleanSupplier robotCentricSup;
+    private BooleanSupplier slowModeSup;
+    private double rotationSpeed;
+    private ProfiledPIDController PID;
+    private DoubleSupplier targetRotation;
+    private boolean isAutoRotating;
+    private static final double AUTO_ROTATE_DEADBAND = 0.3;
+    // private Timer timer;
 
-    public TeleopSwerve(Swerve s_Swerve, DoubleSupplier translationSup, DoubleSupplier strafeSup, DoubleSupplier rotationSup, BooleanSupplier robotCentricSup) {
+    public TeleopSwerve(Swerve s_Swerve, DoubleSupplier translationSup, DoubleSupplier strafeSup, 
+            DoubleSupplier rotationSup, BooleanSupplier robotCentricSup, DoubleSupplier targetRotation,
+            BooleanSupplier slowModeSup, double rotationSpeed, boolean isAutoRotating) {
         this.s_Swerve = s_Swerve;
         addRequirements(s_Swerve);
 
         this.translationSup = translationSup;
         this.strafeSup = strafeSup;
         this.rotationSup = rotationSup;
         this.robotCentricSup = robotCentricSup;
+        this.slowModeSup = slowModeSup;
+        this.rotationSpeed = rotationSpeed;
+        this.targetRotation = targetRotation;
+        this.isAutoRotating = isAutoRotating;
+
+        PID = new ProfiledPIDController(
+            Constants.ROTATE_KP, 
+            Constants.ROTATE_KI, 
+            Constants.ROTATE_KD, 
+            new Constraints(Constants.ROTATE_VELOCITY, Constants.ROTATE_ACCELERATION)
+        ); 
+
+        // timer = new Timer();
     }
 
     @Override
     public void execute() {
         /* Get Values, Deadband*/
         double translationVal = MathUtil.applyDeadband(translationSup.getAsDouble(), Constants.stickDeadband);
-        double strafeVal = MathUtil.applyDeadband(strafeSup.getAsDouble(), Constants.stickDeadband);
+        double strafeVal = MathUtil.applyDeadband(strafeSup.getAsDouble(), Constants.stickDeadband) * rotationSpeed;
         double rotationVal = MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband);
 
+        /*slowmode*/
+        if (slowModeSup.getAsBoolean()) {
+            translationVal = translationVal * Constants.SLOW_MODE_PERCENT_TRANSLATION;
+            strafeVal = strafeVal * Constants.SLOW_MODE_PERCENT_STRAFE;
+            rotationVal = rotationVal * Constants.SLOW_MODE_PERCENT_ROTATION;
+        }
+
+        /* Rotate to Score */
+        if (-AUTO_ROTATE_DEADBAND <= rotationVal && rotationVal <= AUTO_ROTATE_DEADBAND && isAutoRotating) {
+            double yaw = s_Swerve.getYaw().getDegrees() % 360;
+
+            // convert the yaw from [-360, 360] to [-180, 180]
+            if (yaw > 180) {
+                yaw = yaw - 360;
+            } else if (yaw < -180) {
+                yaw = yaw + 360;
+            }
+
+            // Convert to Rotation2d for rotate command
+            Rotation2d rotationYaw = Rotation2d.fromDegrees(yaw);
+
+            // Get the target rotationVal as a Rotation2d object
+            Rotation2d targetRotationVal = Rotation2d.fromDegrees(targetRotation.getAsDouble());
+
+            // Rotate rotationYaw to make the 0 of it at the target angle
+            // If targetRotationVal is 180 degrees the rotationYaw's 0 will be pointing towards 180
+            rotationYaw = rotationYaw.rotateBy(targetRotationVal);
+
+            // double error = yaw - targetRotationVal;
+            //SmartDashboard.putNumber("debug/yaw", yaw);
+            // SmartDashboard.putNumber("debug/targetRotationVal", targetRotationVal);
+            // SmartDashboard.putNumber("debug/error", error);
+            // if (Math.abs(error) > 180) {
+            //     yaw = Math.abs(360 - yaw);
+            // }
+            // SmartDashboard.putNumber("debug/calculated yaw", yaw);
+
+            SmartDashboard.putNumber("debug/calculated yaw", rotationYaw.getDegrees());
+            rotationVal = PID.calculate(rotationYaw.getDegrees(), 0.0); 
+        }
+
         /* Drive */
         s_Swerve.drive(
             new Translation2d(translationVal, strafeVal).times(Constants.Swerve.maxSpeed), 

src/main/java/frc/robot/subsystems/Swerve.java

@@ -117,7 +117,7 @@ public void periodic(){
         swerveOdometry.update(getYaw(), getModulePositions());  
 
         for(SwerveModule mod : mSwerveMods){
-            SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Cancoder", mod.getCanCoder().getDegrees());
+            SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Cancoder", mod.getEncoderAngle().getDegrees());
             SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Integrated", mod.getPosition().angle.getDegrees());
             SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond);    
         }