From 5f3e48b0ec8bec0a5469f63f593d8b250086d144 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E2=80=AEZach=20Wilke?=
 <90875734+76842@users.noreply.github.com>
Date: Mon, 6 Dec 2021 16:41:28 -0700
Subject: [PATCH] Encoder offsets.

---
 src/main/java/frc4388/robot/Constants.java             |  5 +++++
 src/main/java/frc4388/robot/RobotMap.java              | 10 ++++++++--
 .../java/frc4388/robot/subsystems/SwerveDrive.java     |  8 ++++----
 3 files changed, 17 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 0e0790a..56547a9 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -46,6 +46,11 @@ public final class Constants {
         public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 0;
         public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 0;
         public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 0;
+        //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;
     }
     public static final class LEDConstants {
         public static final int LED_SPARK_ID = 0;
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
index 1f2f139..6116011 100644
--- a/src/main/java/frc4388/robot/RobotMap.java
+++ b/src/main/java/frc4388/robot/RobotMap.java
@@ -11,6 +11,7 @@ import com.ctre.phoenix.motorcontrol.InvertType;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.PigeonIMU;
+import com.ctre.phoenix.sensors.AbsoluteSensorRange;
 import com.ctre.phoenix.sensors.CANCoder;
 
 import edu.wpi.first.wpilibj.Spark;
@@ -29,6 +30,7 @@ public class RobotMap {
     public RobotMap() {
         configureLEDMotorControllers();
         configureArcadeDriveMotorControllers();
+        configureSwerveMotorControllers();
     }
 
     /* LED Subsystem */
@@ -83,7 +85,11 @@ public class RobotMap {
     public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID);
     public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
     public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
-
-    void configureSwerveMotorController() {
+            
+    void configureSwerveMotorControllers() {
+        leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
+        rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
+        leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
+        rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
     }
 }
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index 3868365..f1f9890 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -76,13 +76,13 @@ public class SwerveDrive extends SubsystemBase
         SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds);
 
         // Front left module state
-        SwerveModuleState leftFront = SwerveModuleState.optimize(moduleStates[0], new Rotation2d(m_leftFrontEncoder.getPosition()));
+        SwerveModuleState leftFront = SwerveModuleState.optimize(moduleStates[0], Rotation2d.fromDegrees(m_leftFrontEncoder.getPosition()));
         // Front right module state
-        SwerveModuleState rightFront = SwerveModuleState.optimize(moduleStates[1], new Rotation2d(m_rightFrontEncoder.getPosition()));
+        SwerveModuleState rightFront = SwerveModuleState.optimize(moduleStates[1], Rotation2d.fromDegrees(m_rightFrontEncoder.getPosition()));
         // Back left module state
-        SwerveModuleState leftBack = SwerveModuleState.optimize(moduleStates[2], new Rotation2d(m_leftBackEncoder.getPosition()));
+        SwerveModuleState leftBack = SwerveModuleState.optimize(moduleStates[2], Rotation2d.fromDegrees(m_leftBackEncoder.getPosition()));
         // Back right module state
-        SwerveModuleState rightBack = SwerveModuleState.optimize(moduleStates[3], new Rotation2d(m_rightBackEncoder.getPosition()));
+        SwerveModuleState rightBack = SwerveModuleState.optimize(moduleStates[3], Rotation2d.fromDegrees(m_rightBackEncoder.getPosition()));
         
         //Set the Wheel motor speeds
         m_leftFrontWheelMotor.set(m_leftFrontSteerMotor.get() + leftFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);