From 398ea4ac825f285017ff67a36b3a65fa6b0a663a Mon Sep 17 00:00:00 2001
From: EdwardFaucher <ef4366675@gmail.com>
Date: Wed, 12 Mar 2025 17:23:59 -0400
Subject: [PATCH] elevateur va mieux

---
 src/main/java/frc/robot/RobotContainer.java   | 25 +++++++++++--------
 .../robot/TunerConstants/TunerConstants.java  |  2 +-
 2 files changed, 16 insertions(+), 11 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 3f810c0..18dcb71 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -63,15 +63,16 @@ public class RobotContainer {
     .withSize(2, 2).withProperties(Map.of("Label position", "LEFT"));
   GenericEntry L1 = layoutauto.add("choix L1",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
   GenericEntry sortirAngle = layoutauto.add("Cote?",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
-  GenericEntry Reculer = layoutauto.add("Reculer",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
+  GenericEntry ReculerB = layoutauto.add("ReculerB",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
+  GenericEntry ReculerR = layoutauto.add("ReculerR",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
   GenericEntry L4 = layoutauto.add("L4",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
   private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
   private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
 
   /* Setting up bindings for necessary control of the swerve drive platform */
-  private final SwerveRequest.RobotCentric drive = new SwerveRequest.RobotCentric()
+  private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
     .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
-    .withDriveRequestType(DriveRequestType.Velocity
+    .withDriveRequestType(DriveRequestType.OpenLoopVoltage
     ); // Use open-loop control for drive motors
 
   private final Telemetry logger = new Telemetry(MaxSpeed);
@@ -141,14 +142,14 @@ public class RobotContainer {
     manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
     manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain, manette1::getLeftX, manette1::getLeftY));
     
-     //Pince manuel
-     pince.setDefaultCommand(new RunCommand(()->{
-      pince.pivote(MathUtil.applyDeadband((manette2.getRightY()*manette2.getRightY()*manette2.getRightY())/3, 0.05));
+    //Pince manuel
+    pince.setDefaultCommand(new RunCommand(()->{
+    pince.pivote(MathUtil.applyDeadband((manette2.getRightY()*manette2.getRightY()*manette2.getRightY())/3, 0.05));
     }, pince));
 
     //Elevateur manuel
     elevateur.setDefaultCommand(new RunCommand(()->{
-      elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY()*manette2.getLeftY()*manette2.getLeftY(), 0.05));
+    elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY()*manette2.getLeftY()*manette2.getLeftY(), 0.05));
     }, elevateur));
 
     //Reset encodeur
@@ -158,11 +159,15 @@ public class RobotContainer {
     public Command getAutonomousCommand() {
       return new SequentialCommandGroup(
         drivetrain.applyRequest(()->
-        drive.withVelocityX(-0.1*MaxSpeed)
+        drive.withVelocityX(0.1*MaxSpeed)
        .withVelocityY(0)
-       .withRotationalRate(0)).unless(()->!Reculer.getBoolean(true)).withTimeout(3.5),
+       .withRotationalRate(0)).unless(()->!ReculerR.getBoolean(true)).withTimeout(3.5),
+       drivetrain.applyRequest(()->
+       drive.withVelocityX(0.1*MaxSpeed)
+      .withVelocityY(0)
+      .withRotationalRate(0)).unless(()->!ReculerB.getBoolean(true)).withTimeout(3.5),
         drivetrain.applyRequest(()->
-         drive.withVelocityX(0.1*MaxSpeed)
+         drive.withVelocityX(-0.1*MaxSpeed)
         .withVelocityY(0)
         .withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(2.35),
      drivetrain.applyRequest(()->
diff --git a/src/main/java/frc/robot/TunerConstants/TunerConstants.java b/src/main/java/frc/robot/TunerConstants/TunerConstants.java
index 8a85a3e..d4223bd 100644
--- a/src/main/java/frc/robot/TunerConstants/TunerConstants.java
+++ b/src/main/java/frc/robot/TunerConstants/TunerConstants.java
@@ -78,7 +78,7 @@ public class TunerConstants {
 
     // Theoretical free speed (m/s) at 12 V applied output;
     // This needs to be tuned to your individual robot
-    public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.21);
+    public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(6);
 
     // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
     // This may need to be tuned to your individual robot