Add support for MK5n.

README.md

@@ -1,6 +1,6 @@
 # 3061-lib </br>
 
-Huskie Robotics, FRC Team 3061's, starter project and library focused on a swerve-based drivetrain. Supports Swerve Drive Specialties (SDS) MK4/MK4i/MK4n swerve modules using devices from Cross the Road Electronics (CTRE): 2 Falcon 500 / Kraken motors and a CTRE CANCoder, a CTRE Pigeon Gyro along with REV Robotics power distribution hub and pneumatics hub. While, due to the hardware abstraction layer, this code can be adapted to other motor controllers, encoders, and gyros as well as different swerve module designs, only CTRE devices and SDS swerve modules are supported "out of the box."
+Huskie Robotics, FRC Team 3061's, starter project and library focused on a swerve-based drivetrain. Supports Swerve Drive Specialties (SDS) MK4/MK4i/MK4n/MK5n swerve modules using devices from Cross the Road Electronics (CTRE): 2 Falcon 500 / Kraken motors and a CTRE CANCoder, a CTRE Pigeon Gyro along with REV Robotics power distribution hub and pneumatics hub. While, due to the hardware abstraction layer, this code can be adapted to other motor controllers, encoders, and gyros as well as different swerve module designs, only CTRE devices and SDS swerve modules are supported "out of the box."
 
 **Is 3061-lib a Good Fit for Your Team?**
 ----

src/main/java/frc/lib/team3061/swerve_drivetrain/swerve/SwerveConstants.java

@@ -183,6 +183,102 @@ public boolean isCanCoderInverted() {
         }
       };
 
+  public static final SwerveConstants MK5N_R1_CONSTANTS =
+      new SwerveConstants() {
+        @Override
+        public double getDriveGearRatio() {
+          return MK5N_R1_DRIVE_GEAR_RATIO;
+        }
+
+        @Override
+        public boolean isDriveMotorInverted() {
+          return MK5N_DRIVE_MOTOR_INVERTED;
+        }
+
+        @Override
+        public double getAngleGearRatio() {
+          return MK5N_ANGLE_GEAR_RATIO;
+        }
+
+        @Override
+        public boolean isAngleMotorInverted() {
+          return MK5N_ANGLE_MOTOR_INVERTED;
+        }
+
+        @Override
+        public boolean isCanCoderInverted() {
+          return MK5N_CAN_CODER_INVERTED;
+        }
+      };
+
+  public static final SwerveConstants MK5N_R2_CONSTANTS =
+      new SwerveConstants() {
+        @Override
+        public double getDriveGearRatio() {
+          return MK5N_R2_DRIVE_GEAR_RATIO;
+        }
+
+        @Override
+        public boolean isDriveMotorInverted() {
+          return MK5N_DRIVE_MOTOR_INVERTED;
+        }
+
+        @Override
+        public double getAngleGearRatio() {
+          return MK5N_ANGLE_GEAR_RATIO;
+        }
+
+        @Override
+        public boolean isAngleMotorInverted() {
+          return MK5N_ANGLE_MOTOR_INVERTED;
+        }
+
+        @Override
+        public boolean isCanCoderInverted() {
+          return MK5N_CAN_CODER_INVERTED;
+        }
+      };
+
+  public static final SwerveConstants MK5N_R3_CONSTANTS =
+      new SwerveConstants() {
+        @Override
+        public double getDriveGearRatio() {
+          return MK5N_R3_DRIVE_GEAR_RATIO;
+        }
+
+        @Override
+        public boolean isDriveMotorInverted() {
+          return MK5N_DRIVE_MOTOR_INVERTED;
+        }
+
+        @Override
+        public double getAngleGearRatio() {
+          return MK5N_ANGLE_GEAR_RATIO;
+        }
+
+        @Override
+        public boolean isAngleMotorInverted() {
+          return MK5N_ANGLE_MOTOR_INVERTED;
+        }
+
+        @Override
+        public boolean isCanCoderInverted() {
+          return MK5N_CAN_CODER_INVERTED;
+        }
+      };
+
+  /* MK5n */
+  private static final double MK5N_R1_DRIVE_GEAR_RATIO =
+      1.0 / ((12.0 / 54.0) * (32.0 / 25.0) * (15.0 / 30.0));
+  private static final double MK5N_R2_DRIVE_GEAR_RATIO =
+      1.0 / ((14.0 / 54.0) * (32.0 / 25.0) * (15.0 / 30.0));
+  private static final double MK5N_R3_DRIVE_GEAR_RATIO =
+      1.0 / ((16.0 / 54.0) * (32.0 / 25.0) * (15.0 / 30.0));
+  private static final boolean MK5N_DRIVE_MOTOR_INVERTED = true;
+  private static final double MK5N_ANGLE_GEAR_RATIO = 287.0 / 11.0;
+  private static final boolean MK5N_ANGLE_MOTOR_INVERTED = true;
+  private static final boolean MK5N_CAN_CODER_INVERTED = false;
+
   /* MK4n L3 Plus */
   private static final double MK4N_L3_PLUS_DRIVE_GEAR_RATIO =
       1 / ((16.0 / 50.0) * (28.0 / 16.0) * (15.0 / 45.0));

src/main/java/frc/robot/RobotContainer.java

@@ -180,10 +180,10 @@ private void createRobotConfig() {
       case ROBOT_DEFAULT:
         config = new DefaultRobotConfig();
         break;
-      case ROBOT_PRACTICE:
+      case ROBOT_PRACTICE, ROBOT_SIMBOT:
         config = new NewPracticeRobotConfig();
         break;
-      case ROBOT_COMPETITION, ROBOT_SIMBOT:
+      case ROBOT_COMPETITION:
         config = new CalypsoRobotConfig();
         break;
       case ROBOT_PRACTICE_BOARD:

src/main/java/frc/robot/configs/NewPracticeRobotConfig.java

@@ -18,27 +18,25 @@
 import frc.lib.team3061.swerve_drivetrain.swerve.SwerveConstants;
 
 public class NewPracticeRobotConfig extends RobotConfig {
-  // 2 mk4n's on the front, 2 mk4is on the back
-
-  private static final int FRONT_LEFT_MODULE_DRIVE_MOTOR = 43;
-  private static final int FRONT_LEFT_MODULE_STEER_MOTOR = 44;
+  private static final int FRONT_LEFT_MODULE_DRIVE_MOTOR = 37;
+  private static final int FRONT_LEFT_MODULE_STEER_MOTOR = 61;
   private static final int FRONT_LEFT_MODULE_STEER_ENCODER = 14;
-  private static final Angle FRONT_LEFT_MODULE_STEER_OFFSET = Rotations.of(0.271484);
+  private static final Angle FRONT_LEFT_MODULE_STEER_OFFSET = Rotations.of(0.382812);
 
-  private static final int FRONT_RIGHT_MODULE_DRIVE_MOTOR = 41;
-  private static final int FRONT_RIGHT_MODULE_STEER_MOTOR = 42;
-  private static final int FRONT_RIGHT_MODULE_STEER_ENCODER = 8;
-  private static final Angle FRONT_RIGHT_MODULE_STEER_OFFSET = Rotations.of(-0.487793 + 0.5);
+  private static final int FRONT_RIGHT_MODULE_DRIVE_MOTOR = 40;
+  private static final int FRONT_RIGHT_MODULE_STEER_MOTOR = 25;
+  private static final int FRONT_RIGHT_MODULE_STEER_ENCODER = 17;
+  private static final Angle FRONT_RIGHT_MODULE_STEER_OFFSET = Rotations.of(0.471680 + 0.5);
 
-  private static final int BACK_LEFT_MODULE_DRIVE_MOTOR = 38;
-  private static final int BACK_LEFT_MODULE_STEER_MOTOR = 37;
-  private static final int BACK_LEFT_MODULE_STEER_ENCODER = 17;
-  private static final Angle BACK_LEFT_MODULE_STEER_OFFSET = Rotations.of(-0.280029);
+  private static final int BACK_LEFT_MODULE_DRIVE_MOTOR = 39;
+  private static final int BACK_LEFT_MODULE_STEER_MOTOR = 60;
+  private static final int BACK_LEFT_MODULE_STEER_ENCODER = 8;
+  private static final Angle BACK_LEFT_MODULE_STEER_OFFSET = Rotations.of(0.120850);
 
-  private static final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 39;
-  private static final int BACK_RIGHT_MODULE_STEER_MOTOR = 40;
+  private static final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 38;
+  private static final int BACK_RIGHT_MODULE_STEER_MOTOR = 59;
   private static final int BACK_RIGHT_MODULE_STEER_ENCODER = 11;
-  private static final Angle BACK_RIGHT_MODULE_STEER_OFFSET = Rotations.of(-0.105957 + 0.5);
+  private static final Angle BACK_RIGHT_MODULE_STEER_OFFSET = Rotations.of(-0.341797 + 0.5);
 
   private static final int GYRO_ID = 3;
 
@@ -234,17 +232,7 @@ public double getDriveKA() {
 
   @Override
   public SwerveConstants getSwerveConstants() {
-    return SwerveConstants.MK4N_L3_PLUS_CONSTANTS;
-  }
-
-  @Override
-  public SwerveConstants getFrontSwerveConstants() {
-    return SwerveConstants.MK4N_L3_PLUS_CONSTANTS;
-  }
-
-  @Override
-  public SwerveConstants getBackSwerveConstants() {
-    return SwerveConstants.MK4I_L3_PLUS_CONSTANTS;
+    return SwerveConstants.MK5N_R2_CONSTANTS;
   }
 
   @Override