Merge branch 'Serializer' into Intake

src/main/java/frc4388/robot/Constants.java

@@ -42,10 +42,15 @@ public final class Constants {
     public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
     public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
     // ofsets are in degrees
-    public static final float LEFT_FRONT_ENCODER_OFFSET = 0;
-    public static final float RIGHT_FRONT_ENCODER_OFFSET = 0;
-    public static final float LEFT_BACK_ENCODER_OFFSET = 0;
-    public static final float RIGHT_BACK_ENCODER_OFFSET = 0;
+    //ofsets are in degrees
+        // public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141;    
+        // public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281;   
+        // public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531;     
+        // public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594;
+        public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0;
+        public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0;
+        public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0;
+        public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0;
 
     // swerve PID constants
     public static final int SWERVE_SLOT_IDX = 0;
@@ -62,6 +67,16 @@ public final class Constants {
     public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
   }
 
+  public static final class SerializerConstants {
+    public static final double SERIALIZER_BELT_SPEED = 1.0; // TODO (currently max power, DO NOT RUN)
+    public static final double SERIALIZER_SHOOTER_BELT_SPEED = 1.0; // TODO (currently max power, DO NOT RUN)
+    
+    // CAN IDs
+    public static final int SERIALIZER_BELT = -1; // TODO
+    public static final int SERIALIZER_SHOOTER_BELT = -1; // TODO
+    public static final int SERIALIZER_BELT_BEAM = -1; // TODO
+  }
+
   public static final class LEDConstants {
     public static final int LED_SPARK_ID = 0;
 

src/main/java/frc4388/robot/RobotContainer.java

@@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
+import frc4388.robot.subsystems.Serializer;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.LEDPatterns;
 import frc4388.utility.controller.IHandController;
@@ -41,6 +42,8 @@ public class RobotContainer {
   );
 
   private final Intake m_robotIntake = new Intake(null, null);
+  private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, m_robotMap.serializerShooterBelt);
+
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
 
   /* Controllers */
@@ -56,8 +59,8 @@ public class RobotContainer {
     /* Default Commands */
     // drives the swerve drive with a two-axis input from the driver controller
     m_robotSwerveDrive.setDefaultCommand(
-        new RunCommand(() -> m_robotSwerveDrive.driveWithInput(getDriverController().getLeftXAxis(),
-            getDriverController().getLeftYAxis(), getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
+        new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(),
+            getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
 
     m_robotIntake.setDefaultCommand(
         new RunCommand(() -> m_robotIntake.runWithTriggers(
@@ -88,6 +91,10 @@ public class RobotContainer {
     new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
         .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
         .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
+    
+    new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
+        .whenPressed(() -> m_robotSerializer.setSerializerStateWithBeam(true, m_robotSerializer.getBeam()))
+        .whenReleased(() -> m_robotSerializer.setSerializerStateWithBeam(false, m_robotSerializer.getBeam()));
   }
 
   /**

src/main/java/frc4388/robot/RobotMap.java

@@ -9,6 +9,7 @@ import com.ctre.phoenix.sensors.CANCoder;
 
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import frc4388.robot.Constants.LEDConstants;
+import frc4388.robot.Constants.SerializerConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 
 /**
@@ -92,4 +93,8 @@ public class RobotMap {
     //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
   }
 
+  /* Serializer Subsystem */
+
+  public final Spark serializerBelt = new Spark(SerializerConstants.SERIALIZER_BELT);
+  public final Spark serializerShooterBelt = new Spark(SerializerConstants.SERIALIZER_SHOOTER_BELT);
 }

src/main/java/frc4388/robot/subsystems/Serializer.java

@@ -0,0 +1,48 @@
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants;
+import edu.wpi.first.wpilibj.DigitalInput;
+
+public class Serializer extends SubsystemBase{
+  private Spark m_serializerBelt;
+  private Spark m_serializerShooterBelt;
+  private DigitalInput m_serializerBeam;
+  private boolean serializerState;
+
+  public Serializer(Spark serializerBelt, Spark serializerShooterBelt) {
+    m_serializerBelt = serializerBelt;
+    m_serializerShooterBelt = serializerShooterBelt;
+    m_serializerBeam = new DigitalInput(Constants.SerializerConstants.SERIALIZER_BELT_BEAM);
+
+    serializerState = false;
+    setSerializerState(serializerState);
+  }
+  public boolean getBeam() {
+    return m_serializerBeam.get();
+  }
+  public void setSerializerStateWithBeam(boolean ctrlbutter, boolean beambroken) {
+    boolean total = ctrlbutter || beambroken;
+    setSerializerState(total);
+  }
+  public void setSerializerState(boolean state) {
+    setSerializerBeltState(state);
+    setSerializerShooterBeltState(state);
+    serializerState = state;
+  }
+
+  public void setSerializerBeltState(boolean state) {
+    double serializerBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_BELT_SPEED : 0.d;
+    m_serializerBelt.set(serializerBeltSpeed);
+  }
+
+  public void setSerializerShooterBeltState(boolean state) {
+    double serializerShooterBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_SHOOTER_BELT_SPEED : 0.d;
+    m_serializerShooterBelt.set(serializerShooterBeltSpeed);
+  }
+  
+  public boolean getSerializerState() {
+    return serializerState;
+  }
+}

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -14,9 +14,11 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.Gains;
+import frc4388.utility.RobotGyro;
 
 public class SwerveDrive extends SubsystemBase {
   SwerveDriveKinematics m_kinematics;
@@ -28,147 +30,157 @@ public class SwerveDrive extends SubsystemBase {
   private WPI_TalonFX m_leftBackWheelMotor;
   private WPI_TalonFX m_rightBackSteerMotor;
   private WPI_TalonFX m_rightBackWheelMotor;
-  private CANCoder m_leftFrontEncoder;
+  private CANCoder m_leftFrontEncoder; 
   private CANCoder m_rightFrontEncoder;
   private CANCoder m_leftBackEncoder;
-  private CANCoder m_rightBackEncoder;
+  private CANCoder m_rightBackEncoder;    
   double halfWidth = SwerveDriveConstants.WIDTH / 2.d;
   double halfHeight = SwerveDriveConstants.HEIGHT / 2.d;
   public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
 
-  Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(halfWidth));
-  Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(-halfWidth));
-  Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth));
-  Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth));
-  // setSwerveGains();
 
+  Translation2d m_frontLeftLocation = 
+      new Translation2d(
+          Units.inchesToMeters(halfHeight), 
+          Units.inchesToMeters(halfWidth));
+  Translation2d m_frontRightLocation =
+       new Translation2d(
+          Units.inchesToMeters(halfHeight), 
+          Units.inchesToMeters(-halfWidth));
+  Translation2d m_backLeftLocation = 
+      new Translation2d(
+          Units.inchesToMeters(-halfHeight), 
+          Units.inchesToMeters(halfWidth));
+  Translation2d m_backRightLocation = 
+      new Translation2d(
+          Units.inchesToMeters(-halfHeight), 
+          Units.inchesToMeters(-halfWidth));
+      //setSwerveGains();
+      
   private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
   public SwerveModule[] modules;
-  public Gyro gyro; // TODO Add Gyro Lol
+  public RobotGyro gyro; //TODO Add Gyro Lol
 
-  public SwerveDrive(WPI_TalonFX leftFrontSteerMotor, WPI_TalonFX leftFrontWheelMotor,
-      WPI_TalonFX rightFrontSteerMotor, WPI_TalonFX rightFrontWheelMotor,
-      WPI_TalonFX leftBackSteerMotor, WPI_TalonFX leftBackWheelMotor,
-      WPI_TalonFX rightBackSteerMotor, WPI_TalonFX rightBackWheelMotor,
-      CANCoder leftFrontEncoder, CANCoder rightFrontEncoder,
-      CANCoder leftBackEncoder, CANCoder rightBackEncoder) {
-    m_leftFrontSteerMotor = leftFrontSteerMotor;
-    m_leftFrontWheelMotor = leftFrontWheelMotor;
-    m_rightFrontSteerMotor = rightFrontSteerMotor;
-    m_rightFrontWheelMotor = rightFrontWheelMotor;
-    m_leftBackSteerMotor = leftBackSteerMotor;
-    m_leftBackWheelMotor = leftBackWheelMotor;
-    m_rightBackSteerMotor = rightBackSteerMotor;
-    m_rightBackWheelMotor = rightBackWheelMotor;
-    m_leftFrontEncoder = leftFrontEncoder;
-    m_rightFrontEncoder = rightFrontEncoder;
-    m_leftBackEncoder = leftBackEncoder;
-    m_rightBackEncoder = rightBackEncoder;
 
-    modules = new SwerveModule[] {
-        new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder), // Front Left
-        new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder), // Front Right
-        new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder), // Back Left
-        new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder) // Back Right
-    };
-    // gyro.reset();
+  public SwerveDrive(WPI_TalonFX leftFrontSteerMotor,WPI_TalonFX leftFrontWheelMotor,WPI_TalonFX rightFrontSteerMotor,WPI_TalonFX rightFrontWheelMotor,
+  WPI_TalonFX leftBackSteerMotor,WPI_TalonFX leftBackWheelMotor,WPI_TalonFX rightBackSteerMotor,WPI_TalonFX rightBackWheelMotor, CANCoder leftFrontEncoder,
+  CANCoder rightFrontEncoder,
+  CANCoder leftBackEncoder,
+  CANCoder rightBackEncoder)
+  {
+      m_leftFrontSteerMotor = leftFrontSteerMotor;
+      m_leftFrontWheelMotor = leftFrontWheelMotor;
+      m_rightFrontSteerMotor = rightFrontSteerMotor;
+      m_rightFrontWheelMotor = rightFrontWheelMotor;
+      m_leftBackSteerMotor = leftBackSteerMotor;
+      m_leftBackWheelMotor = leftBackWheelMotor;
+      m_rightBackSteerMotor = rightBackSteerMotor;
+      m_rightBackWheelMotor = rightBackWheelMotor;
+      m_leftFrontEncoder = leftFrontEncoder; 
+      m_rightFrontEncoder = rightFrontEncoder;
+      m_leftBackEncoder = leftBackEncoder;
+      m_rightBackEncoder = rightBackEncoder; 
+
+      modules = new SwerveModule[] {
+          new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET), // Front Left
+          new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET), // Front Right
+          new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET), // Back Left
+          new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET)  // Back Right
+      };
+      //gyro.reset(); 
+  }
+//https://github.com/ZachOrr/MK3-Swerve-Example
+ /**
+ * Method to drive the robot using joystick info.
+ *
+ * @param xSpeed Speed of the robot in the x direction (forward).
+ * @param ySpeed Speed of the robot in the y direction (sideways).
+ * @param rot Angular rate of the robot.
+ * @param fieldRelative Whether the provided x and y speeds are relative to the field.
+ */
+  public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative)
+  {
+      /*var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s );
+      driveFromSpeeds(speeds);*/
+      double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
+      double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
+      SwerveModuleState[] states =
+          kinematics.toSwerveModuleStates(
+              fieldRelative
+                ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3, gyro.getRotation2d())
+                : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3));
+       SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
+       for (int i = 0; i < states.length; i++) {
+          SwerveModule module = modules[i];
+          SwerveModuleState state = states[i];
+          module.setDesiredState(state);
+  }
+  }
+  //Converts a ChassisSpeed to SwerveModuleStates (targets)
+  public void driveFromSpeeds(ChassisSpeeds speeds)
+  {
+      //https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html 
+      // Convert to module states
+      SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds);
+
+      // Front left module state
+      SwerveModuleState leftFront = SwerveModuleState.optimize(moduleStates[0], Rotation2d.fromDegrees(m_leftFrontEncoder.getPosition()));
+      // Front right module state
+      SwerveModuleState rightFront = SwerveModuleState.optimize(moduleStates[1], Rotation2d.fromDegrees(m_rightFrontEncoder.getPosition()));
+      // Back left module state
+      SwerveModuleState leftBack = SwerveModuleState.optimize(moduleStates[2], Rotation2d.fromDegrees(m_leftBackEncoder.getPosition()));
+      // Back right module state
+      SwerveModuleState rightBack = SwerveModuleState.optimize(moduleStates[3], Rotation2d.fromDegrees(m_rightBackEncoder.getPosition()));
+      
+      //Set the motors
+      setSwerveMotors(leftFront, leftBack, rightFront, rightBack);
   }
 
-  // https://github.com/ZachOrr/MK3-Swerve-Example
-  /**
-   * Method to drive the robot using joystick info.
-   *
-   * @param xSpeed    Speed of the robot in the x direction (forward).
-   * @param ySpeed    Speed of the robot in the y direction (sideways).
-   * @param rot       Angular rate of the robot.
-   * @param fieldRelative Whether the provided x and y speeds are relative to the
-   *            field.
-   */
-  public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
-
-    // var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s );
-    // driveFromSpeeds(speeds);
-
-    double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
-    double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
-    SwerveModuleState[] states = kinematics.toSwerveModuleStates(
-        fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d())
-            : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot));
-    SwerveDriveKinematics.desaturateWheelSpeeds(states,  Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
-    for (int i = 0; i < states.length; i++) {
-      SwerveModule module = modules[i];
-      SwerveModuleState state = states[i];
-      module.setDesiredState(state);
-    }
-  }
-
-  // Converts a ChassisSpeed to SwerveModuleStates (targets)
-  public void driveFromSpeeds(ChassisSpeeds speeds) {
-    // https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html
-    // Convert to module states
-    SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds);
-
-    // Front left module state
-    SwerveModuleState leftFront = SwerveModuleState.optimize(moduleStates[0], Rotation2d.fromDegrees(m_leftFrontEncoder.getPosition()));
-    // Front right module state
-    SwerveModuleState rightFront = SwerveModuleState.optimize(moduleStates[1], Rotation2d.fromDegrees(m_rightFrontEncoder.getPosition()));
-    // Back left module state
-    SwerveModuleState leftBack = SwerveModuleState.optimize(moduleStates[2], Rotation2d.fromDegrees(m_leftBackEncoder.getPosition()));
-    // Back right module state
-    SwerveModuleState rightBack = SwerveModuleState.optimize(moduleStates[3], Rotation2d.fromDegrees(m_rightBackEncoder.getPosition()));
-
-    // Set the motors
-    setSwerveMotors(leftFront, leftBack, rightFront, rightBack);
-  }
- //Sets steering motors to PID values
+  //Sets steering motors to PID values
   public void setSwerveMotors(SwerveModuleState leftFront, SwerveModuleState leftBack, SwerveModuleState rightFront, SwerveModuleState rightBack)
-  {/*
-    //Set the Wheel motor speeds
-    m_leftFrontWheelMotor.set(m_leftFrontSteerMotor.get() + leftFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
-    m_rightFrontWheelMotor.set(m_rightFrontSteerMotor.get() + rightFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
-    m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + leftBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
-    m_rightBackWheelMotor.set(m_rightBackSteerMotor.get() + rightBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
-
-    //PID
-    m_leftFrontSteerMotor.set(TalonFXControlMode.Position, leftFront.angle.getDegrees() * 12000);
-    m_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees() * 12000);
-    m_leftBackSteerMotor.set(TalonFXControlMode.Position, leftBack.angle.getDegrees() * 12000);
-    m_rightBackSteerMotor.set(TalonFXControlMode.Position, rightBack.angle.getDegrees());
-
-    System.out.println("Target: " + leftFront.angle.getDegrees());
-  */}  
+  {
+      /*//Set the Wheel motor speeds
+      m_leftFrontWheelMotor.set(m_leftFrontSteerMotor.get() + leftFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
+      m_rightFrontWheelMotor.set(m_rightFrontSteerMotor.get() + rightFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
+      m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + leftBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
+      m_rightBackWheelMotor.set(m_rightBackSteerMotor.get() + rightBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
+      //PID
+      m_leftFrontSteerMotor.set(TalonFXControlMode.Position, leftFront.angle.getDegrees() * 12000);
+      m_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees() * 12000);
+      m_leftBackSteerMotor.set(TalonFXControlMode.Position, leftBack.angle.getDegrees() * 12000);
+      m_rightBackSteerMotor.set(TalonFXControlMode.Position, rightBack.angle.getDegrees());
+      System.out.println("Target: " + leftFront.angle.getDegrees());*/
+  }    
   
   /*public void setSwerveGains(){
-    m_leftFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
-    m_leftFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    m_leftFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    m_leftFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    m_leftFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-
-    m_rightFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
-    m_rightFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    m_rightFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    m_rightFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    m_rightFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-
-    m_leftBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
-    m_leftBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    m_leftBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    m_leftBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    m_leftBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-
-    m_rightBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
-    m_rightBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    m_rightBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    m_rightBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    m_rightBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    
+      m_leftFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
+      m_leftFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+      m_leftFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+      m_leftFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+      m_leftFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+      m_rightFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
+      m_rightFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+      m_rightFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+      m_rightFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+      m_rightFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+      m_leftBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
+      m_leftBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+      m_leftBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+      m_leftBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+      m_leftBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+      m_rightBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
+      m_rightBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+      m_rightBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+      m_rightBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+      m_rightBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+      
   }*/
 
 
 
   // public void driveFieldRelative(double awayFromStation, double towardLeftBoundary, double rotate)
   // {
-  //   var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary, rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/)
+  //     var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary, 
+  //         rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/)
   // }
 }

src/main/java/frc4388/robot/subsystems/SwerveModule.java

@@ -15,6 +15,7 @@ import com.ctre.phoenix.sensors.CANCoderConfiguration;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.Gains;
@@ -27,8 +28,10 @@ public class SwerveModule extends SubsystemBase {
 
   private static double kEncoderTicksPerRotation = 4096;
 
+
+
   /** Creates a new SwerveModule. */
-  public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder) {
+  public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
     this.driveMotor = driveMotor;
     this.angleMotor = angleMotor;
     this.canCoder = canCoder;
@@ -45,35 +48,32 @@ public class SwerveModule extends SubsystemBase {
     angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
     angleMotor.configAllSettings(angleTalonFXConfiguration);
 
-    /*
-     * TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
-     * 
-     * driveTalonFXConfiguration.slot0.kP = kDriveP;
-     * driveTalonFXConfiguration.slot0.kI = kDriveI;
-     * driveTalonFXConfiguration.slot0.kD = kDriveD;
-     * driveTalonFXConfiguration.slot0.kF = kDriveF;
-     * 
-     * driveMotor.configAllSettings(driveTalonFXConfiguration);
-     */
+    /*TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
+    driveTalonFXConfiguration.slot0.kP = kDriveP;
+    driveTalonFXConfiguration.slot0.kI = kDriveI;
+    driveTalonFXConfiguration.slot0.kD = kDriveD;
+    driveTalonFXConfiguration.slot0.kF = kDriveF;
+    driveMotor.configAllSettings(driveTalonFXConfiguration);*/
 
     CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
-    // CANCODER CONFIG
+    canCoderConfiguration.magnetOffsetDegrees = offset;
     canCoder.configAllSettings(canCoderConfiguration);
   }
 
+
   public Rotation2d getAngle() {
-    // Note: This assumes the CANCoders are setup with the default feedback coefficient and the sesnor value reports degrees.
+    // Note: This assumes the CANCoders are setup with the default feedback coefficient
+    // and the sesnor value reports degrees.
     return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
   }
 
   /**
    * Set the speed + rotation of the swerve module from a SwerveModuleState object
-   * 
-   * @param desiredState - A SwerveModuleState representing the desired new state
-   *                     of the module
+   * @param desiredState - A SwerveModuleState representing the desired new state of the module
    */
   public void setDesiredState(SwerveModuleState desiredState) {
     Rotation2d currentRotation = getAngle();
+    SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
     SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
 
     // Find the difference between our current rotational position + our new rotational position
@@ -86,7 +86,8 @@ public class SwerveModule extends SubsystemBase {
     double desiredTicks = currentTicks + deltaTicks;
     angleMotor.set(TalonFXControlMode.Position, desiredTicks);
 
+
     double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
     driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.SPEED_FEET_PER_SECOND_AT_FULL_POWER);
   }
-}
+}