feat: drivetrain setup

flamingchickens1540 · Dec 8, 2023 · 521adf5 · 521adf5
1 parent caeeffb
commit 521adf5
Show file tree

Hide file tree

Showing 3 changed files with 35 additions and 39 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -34,4 +34,28 @@ public static enum Mode {
     /** Replaying from a log file. */
     REPLAY
   }
+
+  public static class Drivetrain{
+    public static final double[] offsets = new double[]{
+            75.469, // Module 1
+            91.318 - 90,  // Module 2
+            256.729 + 3, // Module 3
+            9.0,     // Module 4
+            239.766, // Module 5
+            32.08,  // Module 6
+            27.861, // Module 7
+            105.011 + 3  // Module 8
+    };
+
+
+    public static final double[] cornerOffsets = new double[]{
+            90, // Front Left
+            90, // Front Right
+            180, // Back Left
+            90 // Back Right
+    };
+
+
+
+  }
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -25,12 +25,7 @@
 import frc.robot.commands.DriveCommands;
 import frc.robot.commands.FeedForwardCharacterization;
 import frc.robot.commands.ShooterCommands;
-import frc.robot.subsystems.drive.Drive;
-import frc.robot.subsystems.drive.GyroIO;
-import frc.robot.subsystems.drive.GyroIOPigeon2;
-import frc.robot.subsystems.drive.ModuleIO;
-import frc.robot.subsystems.drive.ModuleIOSim;
-import frc.robot.subsystems.drive.ModuleIOSparkMax;
+import frc.robot.subsystems.drive.*;
 import frc.robot.subsystems.limelight.Limelight;
 import frc.robot.subsystems.limelight.LimelightIOReal;
 import frc.robot.subsystems.shooter.*;
@@ -67,10 +62,10 @@ public RobotContainer() {
         drive =
             new Drive(
                 new GyroIOPigeon2(),
-                new ModuleIOSparkMax(0),
-                new ModuleIOSparkMax(1),
-                new ModuleIOSparkMax(2),
-                new ModuleIOSparkMax(3));
+                new ModuleIOTalonFX(0, 3),
+                new ModuleIOTalonFX(1, 4),
+                new ModuleIOTalonFX(2, 7),
+                new ModuleIOTalonFX(3, 1));
         flywheel = new Flywheel(new FlywheelIOSparkMax());
         shooter =
             new Shooter(

diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
@@ -25,6 +25,7 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
+import frc.robot.Constants;
 
 /**
  * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and
@@ -61,35 +62,11 @@ public class ModuleIOTalonFX implements ModuleIO {
   private final boolean isTurnMotorInverted = true;
   private final Rotation2d absoluteEncoderOffset;
 
-  public ModuleIOTalonFX(int index) {
-    switch (index) {
-      case 0:
-        driveTalon = new TalonFX(0);
-        turnTalon = new TalonFX(1);
-        cancoder = new CANcoder(2);
-        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
-        break;
-      case 1:
-        driveTalon = new TalonFX(3);
-        turnTalon = new TalonFX(4);
-        cancoder = new CANcoder(5);
-        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
-        break;
-      case 2:
-        driveTalon = new TalonFX(6);
-        turnTalon = new TalonFX(7);
-        cancoder = new CANcoder(8);
-        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
-        break;
-      case 3:
-        driveTalon = new TalonFX(9);
-        turnTalon = new TalonFX(10);
-        cancoder = new CANcoder(11);
-        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
-        break;
-      default:
-        throw new RuntimeException("Invalid module index");
-    }
+  public ModuleIOTalonFX(int index, int moduleNumber) {
+      driveTalon = new TalonFX(moduleNumber + 30);
+      turnTalon = new TalonFX(moduleNumber + 20);
+      cancoder = new CANcoder(moduleNumber + 10);
+      absoluteEncoderOffset = new Rotation2d((Constants.Drivetrain.cornerOffsets[index] + Constants.Drivetrain.offsets[moduleNumber])%360); // MUST BE CALIBRATED
 
     var driveConfig = new TalonFXConfiguration();
     driveConfig.CurrentLimits.StatorCurrentLimit = 40.0;