Swerve + Constant

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

@@ -6,17 +6,41 @@ package frc.robot;
 
 /** Add your docs here. */
 public class Constants {
-    //moteur
-    public static int lanceur = 1;
-    public static int lancer2 = 2;
-    public static int roue = 3;
-    public static int etoile = 4;
-    public static int Moteuracc2 = 5;
-    public static int Moteuracc = 6;
-    public static int guideur = 7;
-    // limit switsh
-    public static int guideurhaut = 8;
-    public static int guideurbas = 9;
-    public static int limitacc = 58;
+    // Lanceur
+    public static int lanceur = 10;
+    public static int lancer2 = 13;
+    public static int lancer3 = 14;
+    public static int lancer4 = 15;
+    // Ballayeuse
+    public static int roue = 16;
+    public static int etoile = 19;
+
+    // Accumulateur
+    public static int Moteuracc2 = 20;
+    public static int Moteuracc = 21;
+
+    // Guideur
+    public static int guideur = 22;
+
+    // Swerve
+    public static int AvantDroitDrive = 17;
+    public static int AvantDroitAngle = 18;
+    public static int AvantGaucheDrive = 2;
+    public static int AvantGaucheAngle = 3;
+    public static int ArriereDroitDrive = 11;
+    public static int ArriereDroitAngle = 12;
+    public static int ArriereGaucheDrive = 8;
+    public static int ArriereGaucheAngle = 9;
+
+    // CanCoder
+    public static int AvantDroit = 5;
+    public static int AvantGauche = 4;
+    public static int ArriereDroit = 6;
+    public static int ArriereGauche = 7;
+
+    // Limit switsh
+    public static int guideurhaut = 23;
+    public static int guideurbas = 24;
+    public static int limitacc = 25;
   
 }

src/main/java/frc/robot/subsystem/Drive.java

@@ -4,11 +4,40 @@
 
 package frc.robot.subsystem;
 
+import java.io.File;
+import java.io.IOException;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import swervelib.SwerveDrive;
+import swervelib.parser.SwerveParser;
 
 public class Drive extends SubsystemBase {
   /** Creates a new Drive. */
-  public Drive() {}
+  
+   SwerveDrive swerveDrive;
+  File swerveJsonDirectory = new File(Filesystem.getDeployDirectory(),"swerve");
+  public void drive(double x, double y, double zRotation){
+    swerveDrive.drive(new Translation2d(x*2, y*2), zRotation, true, false);
+  }
+  
+  public Drive() {
+
+     try {
+      this.swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(2);
+    } catch (IOException e) {
+      e.printStackTrace();
+    }
+  }
+
+  public SwerveModulePosition[] distance(){
+    return swerveDrive.getModulePositions();
+  }
+public void reset(){
+
+}
 
   @Override
   public void periodic() {