mode auto

src/main/deploy/pathplanner/autos/DepotTirerMur.auto

@@ -48,15 +48,22 @@
           }
         },
         {
-          "type": "named",
+          "type": "deadline",
           "data": {
-            "name": "Lancer"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "TournerA180"
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lancer"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Aspirer"
+                }
+              }
+            ]
           }
         },
         {

src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto

@@ -48,15 +48,22 @@
           }
         },
         {
-          "type": "named",
+          "type": "deadline",
           "data": {
-            "name": "Lancer"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "TournerAZero"
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lancer"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Aspirer"
+                }
+              }
+            ]
           }
         },
         {

src/main/java/frc/robot/commands/Inverser.java

@@ -19,15 +19,13 @@ public class Inverser extends Command {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    drivetrain.getPigeon2().setYaw(drivetrain.getPigeon2().getYaw().getValueAsDouble()+180);
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(!inverse){
-      drivetrain.getPigeon2().setYaw(drivetrain.getPigeon2().getYaw().getValueAsDouble()+180);
-      inverse = true;
-    }
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java

@@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 import frc.robot.subsystems.LimeLight3;
+import frc.robot.subsystems.Limelight3G;
 
 /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
 public class GrimperMur extends Command {
@@ -57,6 +58,7 @@ public class GrimperMur extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    if(limeLight3.getV()){
     BotPose = limeLight3.getBotPoseBlue();
     botx = BotPose[0];
     boty = BotPose[1];
@@ -96,7 +98,7 @@ public class GrimperMur extends Command {
       }
     }
     }
-    
+    }
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java

@@ -53,7 +53,7 @@ public class LimelighterAuto extends Command {
     double[] BotPose = new double[6];
     System.out.println("e");
     if (limelight3g.getV()) {
-        // BotPose = limelight3g.getBotPoseBlue();
+         BotPose = limelight3g.getBotPoseBlue();
        if (!alliance.isPresent()) {
         return;
       }
@@ -65,25 +65,25 @@ public class LimelighterAuto extends Command {
         x = 11.915394;
        BotPose = limelight3g.getBotPoseRed();
       }
-      botx = BotPose[1];
-      boty = BotPose[0];
-      angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
+      botx = BotPose[0];
+      boty = BotPose[1];
+      angle = BotPose[5];
       calcul = limelight3g.Calcule(botx, x, boty, 4, angle);
-      if(angle > 180){
-        angle -= 360;
-      }
       if(calcul > -5 && calcul < 5){
          drivetrain.setControl(
            drive.withRotationalRate(0));
       }
       else if(calcul > 5){
         drivetrain.setControl(
-           drive.withRotationalRate(0.5*(2*Math.PI)));
+          drive.withRotationalRate(2));
       }
       else if(calcul < -5){
-        drivetrain.setControl(
-           drive.withRotationalRate(-0.5*(2*Math.PI)));
+        drivetrain.setControl(drive.withRotationalRate(-2));
       }
+      // botx = BotPose[1];
+      // boty = BotPose[0];
+      // angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
+      // calcul = limelight3g.Calcule(botx, x, boty, 4, angle);
       // if(calcul < -5 && calcul > -180){
       //   drivetrain.setControl(
       //     drive.withRotationalRate(0.5*(2*Math.PI)));
@@ -92,7 +92,7 @@ public class LimelighterAuto extends Command {
       //   drivetrain.setControl(
       //     drive.withRotationalRate(-0.5*(2*Math.PI)));
       // }
-      // else if(calcul >= 180){
+      // else if(calcul < -5){
       //   drivetrain.setControl(
       //     drive.withRotationalRate(-0.5*(2*Math.PI)));
       // }
@@ -126,6 +126,7 @@ public class LimelighterAuto extends Command {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return calcul > -5 && calcul < 5;
+   return calcul > -5 && calcul < 5;
   }
 }
+