From 6b04bd92d3de866a1fed198b5b948c18b64ca62e Mon Sep 17 00:00:00 2001
From: BenG49 <ben@goldfisher.com>
Date: Fri, 26 Jan 2024 15:05:51 -0500
Subject: [PATCH] Add SparkFlex

---
 .../com/stuypulse/robot/constants/Motors.java | 44 ++++++++++++++++---
 .../robot/subsystems/swerve/SwerveDrive.java  |  4 +-
 .../subsystems/swerve/module/JimModule.java   |  2 +-
 .../subsystems/swerve/module/RetepModule.java |  5 ++-
 4 files changed, 44 insertions(+), 11 deletions(-)

diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java
index 34a613d..66b262a 100644
--- a/src/main/java/com/stuypulse/robot/constants/Motors.java
+++ b/src/main/java/com/stuypulse/robot/constants/Motors.java
@@ -5,11 +5,9 @@
 
 package com.stuypulse.robot.constants;
 
-import com.ctre.phoenix.motorcontrol.NeutralMode;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
-import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
 import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
+import com.revrobotics.CANSparkFlex;
 import com.revrobotics.CANSparkMax;
 import static com.revrobotics.CANSparkMax.IdleMode;
 
@@ -38,7 +36,8 @@ public interface Intake {
     }
 
     public interface Swerve {
-        public CANSparkMaxConfig DRIVE_CONFIG = new CANSparkMaxConfig(false, IdleMode.kBrake, 60);
+        public CANSparkFlexConfig DRIVE_CONFIG = new CANSparkFlexConfig(false, IdleMode.kBrake, 60);
+        public CANSparkMaxConfig JIM_DRIVE_CONFIG = new CANSparkMaxConfig(false, IdleMode.kBrake, 60);
         public CANSparkMaxConfig TURN_CONFIG = new CANSparkMaxConfig(false, IdleMode.kBrake, 60);
     }
 
@@ -75,7 +74,40 @@ public void configure(CANSparkMax motor) {
             motor.setSmartCurrentLimit(CURRENT_LIMIT_AMPS);
             motor.setOpenLoopRampRate(OPEN_LOOP_RAMP_RATE);
             motor.burnFlash();
-         }
-          
+        }
+    }
+
+    public static class CANSparkFlexConfig {
+        public final boolean INVERTED;
+        public final IdleMode IDLE_MODE;
+        public final int CURRENT_LIMIT_AMPS;
+        public final double OPEN_LOOP_RAMP_RATE;
+
+        public CANSparkFlexConfig(
+                boolean inverted,
+                IdleMode idleMode,
+                int currentLimitAmps,
+                double openLoopRampRate) {
+            this.INVERTED = inverted;
+            this.IDLE_MODE = idleMode;
+            this.CURRENT_LIMIT_AMPS = currentLimitAmps;
+            this.OPEN_LOOP_RAMP_RATE = openLoopRampRate;
+        }
+
+        public CANSparkFlexConfig(boolean inverted, IdleMode idleMode, int currentLimitAmps) {
+            this(inverted, idleMode, currentLimitAmps, 0.0);
+        }
+
+        public CANSparkFlexConfig(boolean inverted, IdleMode idleMode) {
+            this(inverted, idleMode, 80);
+        }
+
+        public void configure(CANSparkFlex motor) {
+            motor.setInverted(INVERTED);
+            motor.setIdleMode(IDLE_MODE);
+            motor.setSmartCurrentLimit(CURRENT_LIMIT_AMPS);
+            motor.setOpenLoopRampRate(OPEN_LOOP_RAMP_RATE);
+            motor.burnFlash();
+        }
      }
 }
diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java
index 16a7bed..64f1e7d 100644
--- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java
+++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java
@@ -91,7 +91,7 @@ public void configureAutoBuilder() {
                 Swerve.Motion.XY, 
                 Swerve.Motion.THETA, 
                 Swerve.MAX_MODULE_SPEED.get(), 
-                Swerve.WIDTH, 
+                Math.hypot(Swerve.LENGTH, Swerve.WIDTH), 
                 new ReplanningConfig(true, true)), 
             () -> {
                 var alliance = DriverStation.getAlliance();
@@ -99,7 +99,7 @@ public void configureAutoBuilder() {
                     return alliance.get() == DriverStation.Alliance.Red;
                 }
                 return false;
-            }, 
+            },
             instance
         );
     }
diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/module/JimModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/module/JimModule.java
index 8ce88fa..6b61b0d 100644
--- a/src/main/java/com/stuypulse/robot/subsystems/swerve/module/JimModule.java
+++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/module/JimModule.java
@@ -175,7 +175,7 @@ public JimModule(String id, Translation2d location, int turnCANId, Rotation2d an
 
         targetState = new SwerveModuleState();
 
-        Motors.Swerve.DRIVE_CONFIG.configure(driveMotor);
+        Motors.Swerve.JIM_DRIVE_CONFIG.configure(driveMotor);
         Motors.Swerve.TURN_CONFIG.configure(turnMotor);
     }
 
diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/module/RetepModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/module/RetepModule.java
index 74f0245..4386d6a 100644
--- a/src/main/java/com/stuypulse/robot/subsystems/swerve/module/RetepModule.java
+++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/module/RetepModule.java
@@ -1,6 +1,7 @@
 package com.stuypulse.robot.subsystems.swerve.module;
 
 import com.ctre.phoenix6.hardware.CANcoder;
+import com.revrobotics.CANSparkFlex;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.CANSparkBase.IdleMode;
@@ -40,7 +41,7 @@ public class RetepModule extends AbstractModule {
     private final CANcoder turnEncoder;
 
     // drive
-    private final CANSparkMax driveMotor;
+    private final CANSparkFlex driveMotor;
     private final RelativeEncoder driveEncoder; 
     
     // controllers
@@ -53,7 +54,7 @@ public RetepModule(String id, Translation2d translationOffset, Rotation2d angleO
         this.angleOffset = angleOffset;
         
         turnMotor = new CANSparkMax(turnID, MotorType.kBrushless);
-        driveMotor = new CANSparkMax(driveID, MotorType.kBrushless);
+        driveMotor = new CANSparkFlex(driveID, MotorType.kBrushless);
 
         turnMotor.setIdleMode(IdleMode.kBrake);
         driveMotor.setIdleMode(IdleMode.kBrake);