From 631958e662ebe96f12e030511d86375f83de3566 Mon Sep 17 00:00:00 2001
From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com>
Date: Mon, 6 Jan 2025 14:51:44 -0700
Subject: [PATCH] i love using phonix 6 sweve drivetrain

---
 src/main/java/frc4388/robot/Constants.java | 20 ++++++++++++++++++--
 src/main/java/frc4388/robot/RobotMap.java  | 15 +++++++++++++--
 2 files changed, 31 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index b619905..3c6f252 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -10,6 +10,11 @@ package frc4388.robot;
 import java.util.ArrayList;
 import java.util.List;
 
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.ParentConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants;
+
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import frc4388.utility.CanDevice;
 import frc4388.utility.Gains;
@@ -43,7 +48,7 @@ public final class Constants {
         public static final double SLOW_SPEED = 0.25;
         public static final double FAST_SPEED = 0.5;
         public static final double TURBO_SPEED = 1.0;
-    
+
         // public static List<CanDevice> CAN_DEVICES = new ArrayList<>();
 
         public static final class DefaultSwerveRotOffsets {
@@ -127,7 +132,18 @@ public final class Constants {
         public static final double HEIGHT = 18.5;
         public static final double HALF_WIDTH = WIDTH / 2.d;
         public static final double HALF_HEIGHT = HEIGHT / 2.d;
-    
+        
+        public static final SwerveModuleConstants<ParentConfiguration, ParentConfiguration, ParentConfiguration> FRONT_LEFT = new SwerveModuleConstants<>()
+            .withLocationX(HALF_WIDTH)
+            .withLocationX(HALF_HEIGHT)
+            .withDriveMotorId(2)
+            .withDriveMotorInverted(false)
+            .withSteerMotorId(3)
+            .withSteerMotorInverted(false)
+            .withEncoderId(10)
+            .withEncoderInverted(false)
+            .withEncoderOffset(0.0);
+
         // misc
         public static final int TIMEOUT_MS = 30;
         public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
index a5e29df..d338630 100644
--- a/src/main/java/frc4388/robot/RobotMap.java
+++ b/src/main/java/frc4388/robot/RobotMap.java
@@ -10,6 +10,10 @@ package frc4388.robot;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.swerve.SwerveDrivetrain;
+import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants;
+import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
 
 // import edu.wpi.first.wpilibj.motorcontrol.Spark;
 // import frc4388.robot.Constants.LEDConstants;
@@ -22,8 +26,8 @@ import frc4388.utility.RobotGyro;
  * testing and modularization.
  */
 public class RobotMap {
-    private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
-    public RobotGyro gyro = new RobotGyro(m_pigeon2);
+    // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
+    // public RobotGyro gyro = new RobotGyro(m_pigeon2);
     
     public SwerveModule leftFront;
     public SwerveModule rightFront;
@@ -37,6 +41,13 @@ public class RobotMap {
     /* LED Subsystem */
     // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
 
+    public final SwerveDrivetrain swerveDrivetrain = new SwerveDrivetrain(
+        TalonFX::new, TalonFX::new, CANcoder::new, 
+        new SwerveDrivetrainConstants().withPigeon2Id(SwerveDriveConstants.IDs.DRIVE_PIGEON.id),
+        new SwerveModuleConstants[] {
+        }
+    )
+
     /* Swreve Drive Subsystem */
     public final TalonFX  leftFrontWheel    = new  TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL.id);
     public final TalonFX  leftFrontSteer    = new  TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER.id);