Working MKi4 Swerve

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

@@ -26,9 +26,9 @@ public static final class DriveTrain{
       // Drive SubSys
 
       // Drive Max Speeds
-      public static final double DriveTrainMaxSpd   = 1.85;                  // m/s
+      public static final double DriveTrainMaxSpd   = 5;                  // m/s
       public static final double DriveTrainMaxAccel = 0.35;                  // m/s^2
-      public static final double MaxDriveSubSysRotSpeed = 270*Math.PI/180;    // rad/s
+      public static final double MaxDriveSubSysRotSpeed = 720*Math.PI/180;    // rad/s
       public static final double MaxDriveSubSysRotAccel = 180*Math.PI/180;   // rad/s^2
 
       // Drive Trajectory Max Speeds

src/main/java/frc/robot/Library/DriveTrains/SwerveDrive/SubSys_SwerveDrive.java

@@ -53,6 +53,11 @@ public SubSys_SwerveDrive(SubSys_PigeonGyro gyroSubSys) {
             new MK4i_FalconFalcon_Module("BR", SubSys_SwerveDrive_Constants.BR_constants)
         };
 
+        this.swerveOdometry.resetPosition(
+            new Rotation2d(), 
+            this.swerveModulePositionsInit, 
+            new Pose2d());
+
         this.rotationPt = new Translation2d(0,0);
 
         this.rotateLeftPtCmd_prev = false;
@@ -190,23 +195,36 @@ public void periodic(){
         SmartDashboard.putNumber("FL_SteerSensor_Pos", swerveModules[0].getSteerSensorPos());
         SmartDashboard.putNumber("FL_SteerMotor_Pos", swerveModules[0].getSteerMotorPos());
         SmartDashboard.putNumber("FL_SteerMotor_Angle", swerveModules[0].getSteerAngle().getDegrees());
+        SmartDashboard.putNumber("FL_DriveSensor_Pos", swerveModules[0].getDriveMotorPosition());
+        SmartDashboard.putNumber("FL_DriveWheel_Revs", swerveModules[0].getDriveWheelRevs());
+        SmartDashboard.putNumber("FL_DriveWheel_Distance", swerveModules[0].getDriveMotorDistance());
+
+        SmartDashboard.putNumber("FL_SwrPosition_Distance",swerveModules[0].getPosition().distanceMeters);
+        SmartDashboard.putNumber("FL_SwrPosition_Angle",swerveModules[0].getPosition().angle.getDegrees());
 
         // Front Right (Module 1)
         SmartDashboard.putNumber("FR_SteerSensor_AbsPos", swerveModules[1].getSteerSensorAbsolutePos());
         SmartDashboard.putNumber("FR_SteerSensor_Pos", swerveModules[1].getSteerSensorPos());
         SmartDashboard.putNumber("FR_SteerMotor_Pos", swerveModules[1].getSteerMotorPos());
         SmartDashboard.putNumber("FR_SteerMotor_Angle", swerveModules[1].getSteerAngle().getDegrees());
+        SmartDashboard.putNumber("FR_DriveSensor_Pos", swerveModules[1].getDriveMotorPosition());
 
         // Back Left (Module 2)
         SmartDashboard.putNumber("BL_SteerSensor_AbsPos", swerveModules[2].getSteerSensorAbsolutePos());
         SmartDashboard.putNumber("BL_SteerSensor_Pos", swerveModules[2].getSteerSensorPos());
         SmartDashboard.putNumber("BL_SteerMotor_Pos", swerveModules[2].getSteerMotorPos());
         SmartDashboard.putNumber("BL_SteerMotor_Angle", swerveModules[2].getSteerAngle().getDegrees());
+        SmartDashboard.putNumber("BL_DriveSensor_Pos", swerveModules[2].getDriveMotorPosition());
 
         // Back Right (Module 3)
         SmartDashboard.putNumber("BR_SteerSensor_AbsPos", swerveModules[3].getSteerSensorAbsolutePos());
         SmartDashboard.putNumber("BR_SteerSensor_Pos", swerveModules[3].getSteerSensorPos());
         SmartDashboard.putNumber("BR_SteerMotor_Pos", swerveModules[3].getSteerMotorPos());
         SmartDashboard.putNumber("BR_SteerMotor_Angle", swerveModules[3].getSteerAngle().getDegrees());
+        SmartDashboard.putNumber("BR_DriveSensor_Pos", swerveModules[3].getDriveMotorPosition());
+
+        // Odometry
+        SmartDashboard.putNumber("Xdistance", swerveOdometry.getPoseMeters().getX());
+        SmartDashboard.putNumber("Ydistance", swerveOdometry.getPoseMeters().getY());
     }
 }

src/main/java/frc/robot/Library/DriveTrains/SwerveDrive/SubSys_SwerveDrive_Constants.java

@@ -38,18 +38,18 @@ public class SubSys_SwerveDrive_Constants {
         Constants.CAN_IDs.FrontLeftSteerMtr_CAN_ID,             // Steer Motor CAN ID
         true,                                 // Steer Motor Inverted   
         Constants.CAN_IDs.FrontLeftSteerCANCoder_CAN_ID,        // CANCoder ID (255 for not used)       
-        true,
-        0);                                      // Degrees
+        false,
+        182.8);                                      // Degrees
 
     public static final SwerveModuleConstants FR_constants = new SwerveModuleConstants(
         "FR",                                       // Module Name
         Constants.CAN_IDs.FrontRightDriveMtr_CAN_ID,    // Drive Motor CAN ID
-        false,                                      // Drive Motor Inverted
+        true,                                      // Drive Motor Inverted
         Constants.CAN_IDs.FrontRightSteerMtr_CAN_ID,    // Steer Motor CAN ID
         true,                                      // Steer Motor Inverted   
         Constants.CAN_IDs.FrontRightSteerCANCoder_CAN_ID,                                        // CANCoder ID (255 for not used)       
-        true,
-        289);                                      // Degrees 
+        false,
+        289.5);                                      // Degrees 
 
     public static final SwerveModuleConstants BL_constants = new SwerveModuleConstants(
         "BL",                                       // Module Name
@@ -58,18 +58,18 @@ public class SubSys_SwerveDrive_Constants {
         Constants.CAN_IDs.BackLeftSteerMtr_CAN_ID,      // Steer Motor CAN ID
         true,                                      // Steer Motor Inverted   
         Constants.CAN_IDs.BackLeftSteerCANCoder_CAN_ID,                                        // CANCoder ID (255 for not used)       
-        true,
-        166);                                      // Degrees
+        false,
+        168);                                      // Degrees
 
     public static final SwerveModuleConstants BR_constants = new SwerveModuleConstants(
         "BR",                                       // Module Name
         Constants.CAN_IDs.BackRightDriveMtr_CAN_ID,     // Drive Motor CAN ID
-        false,                                      // Drive Motor Inverted
+        true,                                      // Drive Motor Inverted
         Constants.CAN_IDs.BackRightSteerMtr_CAN_ID,     // Steer Motor CAN ID
         true,                                      // Steer Motor Inverted   
         Constants.CAN_IDs.BackRightSteerCANCoder_CAN_ID,                                        // CANCoder ID (255 for not used)       
-        true,
-        106);                                      // Degrees
+        false,
+        106.5);                                      // Degrees
 
     public static final Translation2d RotationPtFL = new Translation2d(
         0.6096,

src/main/java/frc/robot/Library/DriveTrains/SwerveDrive/SwerveModules/MK4i_FalconFalcon/MK4i_FalconFalcon_Module.java

@@ -30,6 +30,7 @@
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants;
 import frc.robot.Library.DriveTrains.SwerveDrive.SwerveModules.CTREModuleState;
 import frc.robot.Library.DriveTrains.SwerveDrive.SwerveModules.SwerveModuleConstants;
@@ -43,7 +44,7 @@ public class MK4i_FalconFalcon_Module {
     private TalonFX driveMotor;
     private TalonFX steerMotor;
     private CANCoder steerAngleEncoder;
-    private double strAngleOffset;  
+    private double strZeroAngle;  
     private double lastAngle;
 
     //public int moduleNumber;
@@ -63,6 +64,7 @@ public MK4i_FalconFalcon_Module(
         SwerveModuleConstants moduleConstants)
         {
             this.moduleName = moduleName;
+            this.strZeroAngle = moduleConstants.zeroAngle;
 
             /* Drive Motor Config */
             this.driveMotor = new TalonFX(moduleConstants.driveMotorID);
@@ -75,8 +77,6 @@ public MK4i_FalconFalcon_Module(
             /* Steer Motor Config */
             this.steerMotor = new TalonFX(moduleConstants.steerMotorID);
             configSteerMotor(moduleConstants);
-
-            this.strAngleOffset = moduleConstants.angleOffset;
 
             lastAngle = getState().angle.getDegrees();
         }
@@ -157,16 +157,13 @@ public SwerveModuleState getState(){
     */
       public SwerveModulePosition getPosition(){
         SwerveModulePosition swrModulePosition;
-        double drvDistance;
-        Rotation2d strAngle;
-
-        drvDistance = driveMotor.getSelectedSensorPosition();
-        strAngle = getSteerAngle();
-
-    swrModulePosition = new SwerveModulePosition(drvDistance, strAngle);
+
+        swrModulePosition = new SwerveModulePosition(
+            getDriveMotorDistance(),
+            getSteerAngle());
 
-    return swrModulePosition;
-  }
+      return swrModulePosition;
+    }
 
     /* ***** Drive Motor Methods ***** */  
 
@@ -177,6 +174,38 @@ private void configDriveMotor(SwerveModuleConstants moduleConstants){
         driveMotor.setNeutralMode(MK4i_FalconFalcon_Module_Constants.DriveMotor.neutralMode);
         //driveMotor.setSelectedSensorPosition(0);
     }
+
+    /** getDriveMotorPosition
+     *   Get Drive Motor Sensor Position
+     * @return double Integrated Sensor Counts
+     */
+    public double getDriveMotorPosition(){
+        return driveMotor.getSelectedSensorPosition();
+    }
+
+    /** getDriveWheelRevs
+     *   Get Drive Motor 
+     * @return double Drive Wheel Revs
+     */
+    public double getDriveWheelRevs(){
+
+        double wheelRevs = TalonFX_Conversions.talonFXToRevs_GearRatio(
+            getDriveMotorPosition(),
+            MK4i_FalconFalcon_Module_Constants.DriveMotor.invDriveGearRatio);
+        return wheelRevs;
+    }
+
+    /** getDriveWheelDistance
+     *   Get Drive Wheel Distance
+     * @return double Drive Wheel Distance (m)
+     */
+    public double getDriveMotorDistance(){
+        double wheelRevs = TalonFX_Conversions.talonFXToRevs_GearRatio(
+            getDriveMotorPosition(),
+            MK4i_FalconFalcon_Module_Constants.DriveMotor.invDriveGearRatio);
+        return wheelRevs*MK4i_FalconFalcon_Module_Constants.DriveMotor.driveWheelCircumference;
+    }
+
 
     /* ***** Steer Angle Encoder Methods ***** */
 
@@ -187,7 +216,10 @@ private void configDriveMotor(SwerveModuleConstants moduleConstants){
     private void configSteerAngleEncoder(SwerveModuleConstants moduleConstants){
         steerAngleEncoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition);
         steerAngleEncoder.configSensorDirection(moduleConstants.SteerCANcoderInvert);
-        steerAngleEncoder.setPosition(steerAngleEncoder.getAbsolutePosition()-this.strAngleOffset);
+        double strAngleInit = steerAngleEncoder.getAbsolutePosition()-this.strZeroAngle;
+        SmartDashboard.putNumber("strAngleInit", strAngleInit);
+        //steerAngleEncoder.setPosition((steerAngleEncoder.getAbsolutePosition()-this.strZeroAngle));
+        steerAngleEncoder.setPosition(strAngleInit);
     }
 
     /** getSteerSensorAbsolutePos
@@ -236,6 +268,8 @@ private void configSteerMotor(SwerveModuleConstants moduleConstants){
             FeedbackDevice.RemoteSensor0, 
             0, 
             0);
+
+        steerMotor.config_kP(0, 0.6, 0);
     }
 
     /** getSteerMotorPos

src/main/java/frc/robot/Library/DriveTrains/SwerveDrive/SwerveModules/MK4i_FalconFalcon/MK4i_FalconFalcon_Module_Constants.java

@@ -68,14 +68,15 @@ public MK4i_FalconFalcon_Module_Constants(){
     public static final class DriveMotor {
 
         /** Neutral Mode */
-        public static final NeutralMode neutralMode = NeutralMode.Brake; 
+        public static final NeutralMode neutralMode = NeutralMode.Coast; 
 
         /** Drive Motor Wheel Circumference  */
-        public static final double driveWheelDiameter = Units.inchesToMeters(3.94);  // meters
+        public static final double driveWheelDiameter = Units.inchesToMeters(4.0);  // meters
         public static final double driveWheelCircumference = driveWheelDiameter * Math.PI; // meters
 
         /** Drive Motor Gear Ratio */
-        public static final double driveGearRatio = (6.86 / 1.0); //6.86:1
+        public static final double driveGearRatio = (6.75);             // 6.75:1
+        public static final double invDriveGearRatio = (0.148148148);   // 1:6.75
 
         /** Feedforward Characterization Values */
         public static final double driveKS = (0.667 / 12); //divide by 12 to convert from volts to percent output for CTRE

src/main/java/frc/robot/Library/DriveTrains/SwerveDrive/SwerveModules/SwerveModuleConstants.java

@@ -13,7 +13,7 @@ public class SwerveModuleConstants {
     public final boolean steerMotorInvert;
     public final int cancoderID;
     public final boolean SteerCANcoderInvert;
-    public final double angleOffset;
+    public final double zeroAngle;
 
     /**
      * Swerve Module Constants to be used when creating swerve modules.
@@ -24,7 +24,7 @@ public class SwerveModuleConstants {
      * @param steerMotorInvert
      * @param canCoderID
      * @param steerCANCoderInvert
-     * @param angleOffset
+     * @param zeroAngle
      */
     public SwerveModuleConstants(
         String moduleName,
@@ -34,7 +34,7 @@ public SwerveModuleConstants(
         boolean steerMotorInvert,
         int canCoderID,
         boolean steerCANCoderInvert,
-        double angleOffset){
+        double zeroAngle){
 
             this.moduleName = moduleName;
             this.driveMotorID = driveMotorID;
@@ -43,6 +43,6 @@ public SwerveModuleConstants(
             this.steerMotorInvert = steerMotorInvert;
             this.cancoderID = canCoderID;
             this.SteerCANcoderInvert = steerCANCoderInvert;
-            this.angleOffset = angleOffset;
+            this.zeroAngle = zeroAngle;
     }
 }

src/main/java/frc/robot/Library/DriveTrains/SwerveDrive/SwerveModules/SwrNStr_FalconPG71/SwrNStr_FalconPG71_Module.java

@@ -65,7 +65,7 @@ public SwrNStr_FalconPG71_Module(
             //configSteerMotor(moduleConstants);
 
 
-            this.strAngleOffset = moduleConstants.angleOffset;
+            this.strAngleOffset = moduleConstants.zeroAngle;
 
             /* Angle Encoder Config */
             //angleEncoder = new CANCoder(moduleConstants.cancoderID);

src/main/java/frc/robot/Library/MotorControllers/TalonFX/TalonFX_Conversions.java

@@ -7,6 +7,9 @@
 /** Add your docs here. */
 public class TalonFX_Conversions {
 
+    public static final double TALONFX_CNTS_TO_REVS = 0.00048828125;   // 1/2048
+    public static final double REVS_TO_TALONFX_CNTS = 2048;
+
     /** talonFXToDegrees
      * @param counts TalonFX Counts
      * @return Degrees of Rotation of Mechanism
@@ -47,6 +50,31 @@ public static double degreesToTalonFX_wGearRatio(double degrees, double gearRati
         return counts;
     }
 
+    /** talonFXToRevs
+     *   Convert from TalonFX Sensor Counts to Revolutions
+     * @param counts double TalonFX Sensor Counts
+     * @return double Revolutions
+     */
+    public static double talonFXToRevs (double counts){
+        return counts*TALONFX_CNTS_TO_REVS;
+    }
+
+    /** talonFXToRevs_GearRatio
+     *   Convert from TalonFX Sensor Counts to Revolutions
+     * @param counts double TalonFX Sensor Counts
+     * @return double Revolutions
+    */
+
+    /** talonFXToRevs_GearRatio
+    *     Convert from TalonFX Counts to Revolutions through a gearbox
+    * @param counts double TalonFX Sensor Counts
+    * @param gearRatio double GearRatio
+    * @return double Revolutions
+    */
+    public static double talonFXToRevs_GearRatio (double counts, double gearRatio){
+        return counts*TALONFX_CNTS_TO_REVS*gearRatio;
+    }
+
     /**
      * @param velocityCounts Falcon Velocity Counts
      * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM)