YAGLS

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

@@ -4,12 +4,46 @@
 
 package frc.robot.Subsystems;
 
+import java.io.File;
+import java.io.IOException;
+
+import com.ctre.phoenix6.hardware.Pigeon2;
+
+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");
+  
+  Pigeon2 Gyro = new Pigeon2(0);
+   
+  public void drive(double x, double y, double zRotation){
+    swerveDrive.drive(new Translation2d(x*5, y*5), zRotation*4, true, false);
+  }
+  
+  public Drive() {
+     try {
+      this.swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(5);
+      swerveDrive.setHeadingCorrection(true);
+    } catch (IOException e) {
+      e.printStackTrace();
+    }
+  }
+  public void restgyroscope(){
+    Gyro.reset();
+  }
+  public SwerveModulePosition[] distance(){
+    return swerveDrive.getModulePositions();
+  }
+
   
   @Override
   public void periodic() {