diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5acd59c..bffe940 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -73,7 +74,12 @@ public class RobotContainer { JoystickButton buttonX = new JoystickButton(manette, XboxController.Button.kX.value); buttonX.whenPressed(ActiverBlockeur); JoystickButton buttonstart = new JoystickButton(manette, XboxController.Button.kStart.value); - buttonstart.whenPressed(resetGyro); + buttonstart.whenPressed( + new InstantCommand( + () -> basePilotable.resetgyro(), + basePilotable + ) + ); } /** diff --git a/src/main/java/frc/robot/commands/ResetGyro.java b/src/main/java/frc/robot/commands/ResetGyro.java index b209286..f602f87 100644 --- a/src/main/java/frc/robot/commands/ResetGyro.java +++ b/src/main/java/frc/robot/commands/ResetGyro.java @@ -4,7 +4,6 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.BasePilotable; @@ -20,12 +19,7 @@ public class ResetGyro extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { - try { - basePilotable.resetgyro(); - } catch( Exception e) { - DriverStation.reportError("Samuel", e.getStackTrace()); - } - + } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/subsystems/BasePilotable.java b/src/main/java/frc/robot/subsystems/BasePilotable.java index 1bac661..093c083 100644 --- a/src/main/java/frc/robot/subsystems/BasePilotable.java +++ b/src/main/java/frc/robot/subsystems/BasePilotable.java @@ -32,7 +32,12 @@ public void drive(double y, double x, double rot){ mecanum.driveCartesian(y, rot, x, -getangle()); } public void resetgyro(){ -gyroscope.zeroYaw(); + try { + gyroscope.zeroYaw(); + } catch( Exception e) { + e.printStackTrace(); + } + } /** Creates a new BasePilotable. */ public BasePilotable() {