From 40b23cf3f1362e300d7c7275a784cc6622f8bc26 Mon Sep 17 00:00:00 2001
From: samuel desharnais <samdes330@gmail.com>
Date: Tue, 27 Feb 2024 18:04:23 -0500
Subject: [PATCH]

---
 src/main/java/frc/robot/RobotContainer.java   |  2 +
 src/main/java/frc/robot/command/RestGyro.java | 38 +++++++++++++++++++
 src/main/java/frc/robot/subsystem/Drive.java  |  8 +++-
 3 files changed, 46 insertions(+), 2 deletions(-)
 create mode 100644 src/main/java/frc/robot/command/RestGyro.java

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index f62d9d0..ab16101 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -32,6 +32,7 @@ import frc.robot.command.Lancerampli;
 import frc.robot.command.Limelight_tracker;
 import frc.robot.command.PistonFerme;
 import frc.robot.command.PistonOuvre;
+import frc.robot.command.RestGyro;
 // Subsystems
 import frc.robot.subsystem.Accumulateur;
 import frc.robot.subsystem.Balayeuse;
@@ -95,6 +96,7 @@ public class RobotContainer {
     joystick.button(2).whileTrue(new ParallelCommandGroup(new Lancerampli(lanceur,limelight),new Limelight_tracker(limelight,drive)));
     joystick.button(6).whileTrue(new LancerTrape(lanceur, accumulateur));
     joystick.button(7).whileTrue(new Limelight_tracker(limelight, drive));
+    manette.start().whileTrue(new RestGyro(drive));
     // deplacement
     configureBindings();
       drive.setDefaultCommand(new RunCommand(()->{
diff --git a/src/main/java/frc/robot/command/RestGyro.java b/src/main/java/frc/robot/command/RestGyro.java
new file mode 100644
index 0000000..0c2c3fc
--- /dev/null
+++ b/src/main/java/frc/robot/command/RestGyro.java
@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.command;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystem.Drive;
+
+public class RestGyro extends Command {
+  private Drive drive;
+  /** Creates a new RestGyro. */
+  public RestGyro(Drive drive) {
+    this.drive = drive;
+    addRequirements(drive);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    drive.restgyroscope();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/subsystem/Drive.java b/src/main/java/frc/robot/subsystem/Drive.java
index 4b4eab7..d54968a 100644
--- a/src/main/java/frc/robot/subsystem/Drive.java
+++ b/src/main/java/frc/robot/subsystem/Drive.java
@@ -6,6 +6,8 @@ package frc.robot.subsystem;
 
 import java.io.File;
 import java.io.IOException;
+
+import com.kauailabs.navx.frc.AHRS;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PIDConstants;
@@ -23,7 +25,7 @@ public class Drive extends SubsystemBase {
    SwerveDrive swerveDrive;
 
   File swerveJsonDirectory = new File(Filesystem.getDeployDirectory(),"swerve");
-  
+  public AHRS gyroscope = new AHRS();
   public void drive(double x, double y, double zRotation){
     swerveDrive.drive(new Translation2d(x*5, y*5), zRotation*3, true, false);
   }
@@ -46,7 +48,9 @@ public class Drive extends SubsystemBase {
         return false;
          }, this);
   }
-
+  public void restgyroscope(){
+    gyroscope.reset();
+  }
   public SwerveModulePosition[] distance(){
     return swerveDrive.getModulePositions();
   }