Added Encoders

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

@@ -41,6 +41,10 @@ public final class Constants {
         public static final int LEFT_BACK_WHEEL_CAN_ID = 0;
         public static final int RIGHT_BACK_STEER_CAN_ID = 0;
         public static final int RIGHT_BACK_WHEEL_CAN_ID = 0;
+        public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 0;
+        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;
     }
     public static final class LEDConstants {
         public static final int LED_SPARK_ID = 0;

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.CANCoder;
 
 import edu.wpi.first.wpilibj.Spark;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -78,4 +79,11 @@ public class RobotMap {
     public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID);
     public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID);
     public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID);
+    public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID);
+    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() {
+    }
 }

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

@@ -8,6 +8,7 @@
 package frc4388.robot.subsystems;
 
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix.sensors.CANCoder;
 
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Translation2d;
@@ -27,8 +28,15 @@ public class SwerveDrive
     private WPI_TalonFX m_leftBackWheelMotor;
     private WPI_TalonFX m_rightBackSteerMotor;
     private WPI_TalonFX m_rightBackWheelMotor;
+    private CANCoder m_leftFrontEncoder; 
+    private CANCoder m_rightFrontEncoder;
+    private CANCoder m_leftBackEncoder;
+    private CANCoder m_rightBackEncoder;    
     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)
+    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;
@@ -38,6 +46,10 @@ public class SwerveDrive
         m_leftBackWheelMotor = leftBackWheelMotor;
         m_rightBackSteerMotor = rightBackSteerMotor;
         m_rightBackWheelMotor = rightBackWheelMotor;
+        m_leftFrontEncoder = leftFrontEncoder; 
+        m_rightFrontEncoder = rightFrontEncoder;
+        m_leftBackEncoder = leftBackEncoder;
+        m_rightBackEncoder = rightBackEncoder; 
         double halfWidth = SwerveDriveConstants.WIDTH / 2.d;
         double halfHeight = SwerveDriveConstants.HEIGHT / 2.d;
 
@@ -57,16 +69,16 @@ public class SwerveDrive
         SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds);
 
         // Front left module state
-        SwerveModuleState frontLeft = SwerveModuleState.optimize(moduleStates[0], new Rotation2d(/*get encoder positions*/));
+        SwerveModuleState leftFront = SwerveModuleState.optimize(moduleStates[0], new Rotation2d(m_leftFrontEncoder.getPosition()));
 
         // Front right module state
-        SwerveModuleState frontRight = SwerveModuleState.optimize(moduleStates[1], new Rotation2d(/*get encoder positions*/));
+        SwerveModuleState rightFront = SwerveModuleState.optimize(moduleStates[1], new Rotation2d(m_rightFrontEncoder.getPosition()));
 
         // Back left module state
-        SwerveModuleState backLeft = SwerveModuleState.optimize(moduleStates[2], new Rotation2d(/*get encoder positions*/));
+        SwerveModuleState lefBack = SwerveModuleState.optimize(moduleStates[2], new Rotation2d(m_leftBackEncoder.getPosition()));
 
         // Back right module state
-        SwerveModuleState backRight = SwerveModuleState.optimize(moduleStates[3], new Rotation2d(/*get encoder positions*/));
+        SwerveModuleState rightBack = SwerveModuleState.optimize(moduleStates[3], new Rotation2d(m_rightBackEncoder.getPosition()));
 
     }