mode auto

src/main/deploy/pathplanner/autos/Copy of TirSimple.auto

@@ -0,0 +1,31 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TirerSimple"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "DescendreBalayeuse"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Lancer"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -4,6 +4,12 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "DescendreBalayeuse"
+          }
+        },
         {
           "type": "deadline",
           "data": {
@@ -14,12 +20,6 @@
                   "pathName": "Depot"
                 }
               },
-              {
-                "type": "named",
-                "data": {
-                  "name": "DescendreBalayeuse"
-                }
-              },
               {
                 "type": "named",
                 "data": {

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

@@ -4,6 +4,12 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "DescendreBalayeuse"
+          }
+        },
         {
           "type": "deadline",
           "data": {
@@ -14,12 +20,6 @@
                   "pathName": "Depot"
                 }
               },
-              {
-                "type": "named",
-                "data": {
-                  "name": "DescendreBalayeuse"
-                }
-              },
               {
                 "type": "named",
                 "data": {

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

@@ -4,6 +4,12 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "DescendreBalayeuse"
+          }
+        },
         {
           "type": "deadline",
           "data": {
@@ -14,12 +20,6 @@
                   "pathName": "Depot"
                 }
               },
-              {
-                "type": "named",
-                "data": {
-                  "name": "DescendreBalayeuse"
-                }
-              },
               {
                 "type": "named",
                 "data": {

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

@@ -4,6 +4,12 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "DescendreBalayeuse"
+          }
+        },
         {
           "type": "deadline",
           "data": {
@@ -14,12 +20,6 @@
                   "pathName": "ChercherMilieuDroite"
                 }
               },
-              {
-                "type": "named",
-                "data": {
-                  "name": "DescendreBalayeuse"
-                }
-              },
               {
                 "type": "named",
                 "data": {

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

@@ -4,6 +4,12 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "DescendreBalayeuse"
+          }
+        },
         {
           "type": "deadline",
           "data": {
@@ -14,12 +20,6 @@
                   "pathName": "ChercherMilieuDroiteProche"
                 }
               },
-              {
-                "type": "named",
-                "data": {
-                  "name": "DescendreBalayeuse"
-                }
-              },
               {
                 "type": "named",
                 "data": {

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

@@ -4,6 +4,12 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "DescendreBalayeuse"
+          }
+        },
         {
           "type": "deadline",
           "data": {
@@ -14,12 +20,6 @@
                   "pathName": "ChercherMilieuGauche"
                 }
               },
-              {
-                "type": "named",
-                "data": {
-                  "name": "DescendreBalayeuse"
-                }
-              },
               {
                 "type": "named",
                 "data": {

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

@@ -4,6 +4,12 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "DescendreBalayeuse"
+          }
+        },
         {
           "type": "deadline",
           "data": {
@@ -14,12 +20,6 @@
                   "pathName": "ChercherMilieuGaucheProche"
                 }
               },
-              {
-                "type": "named",
-                "data": {
-                  "name": "DescendreBalayeuse"
-                }
-              },
               {
                 "type": "named",
                 "data": {

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

@@ -10,6 +10,12 @@
             "pathName": "TirerSimple"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "DescendreBalayeuse"
+          }
+        },
         {
           "type": "named",
           "data": {

src/main/deploy/pathplanner/settings.json

@@ -1,6 +1,6 @@
 {
-  "robotWidth": 0.9,
+  "robotWidth": 0.83,
-  "robotLength": 0.9,
+  "robotLength": 0.87,
   "holonomicMode": true,
   "pathFolders": [
     "Milieu"
@@ -13,23 +13,23 @@
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
-  "robotMass": 74.088,
+  "robotMass": 54.5,
   "robotMOI": 6.883,
   "robotTrackwidth": 0.546,
-  "driveWheelRadius": 0.048,
+  "driveWheelRadius": 0.1,
   "driveGearing": 5.143,
-  "maxDriveSpeed": 5.45,
+  "maxDriveSpeed": 5.374,
   "driveMotorType": "krakenX60",
   "driveCurrentLimit": 60.0,
   "wheelCOF": 1.2,
-  "flModuleX": 0.273,
+  "flModuleX": 0.288925,
-  "flModuleY": 0.273,
+  "flModuleY": 0.269875,
-  "frModuleX": 0.273,
+  "frModuleX": 0.288925,
-  "frModuleY": -0.273,
+  "frModuleY": -0.27,
-  "blModuleX": -0.273,
+  "blModuleX": -0.288925,
-  "blModuleY": 0.273,
+  "blModuleY": 0.269875,
-  "brModuleX": -0.273,
+  "brModuleX": -0.288925,
-  "brModuleY": -0.273,
+  "brModuleY": -0.27,
   "bumperOffsetX": 0.0,
   "bumperOffsetY": 0.0,
   "robotFeatures": []

src/main/java/frc/robot/RobotContainer.java

@@ -14,6 +14,7 @@ import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
@@ -72,12 +73,9 @@ public class RobotContainer {
 
 
   public RobotContainer() {
-    autoChooser = AutoBuilder.buildAutoChooser();
-    CameraServer.startAutomaticCapture();
-    configureBindings();
     NamedCommands.registerCommand("GrimperMur", new GrimperMur(limeLight3,drivetrain));
     NamedCommands.registerCommand("GrimperReservoir", new GrimperReservoir(limeLight3G,drivetrain));
-    NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur));
+    NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur, limeLight3G));
     NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite());
     NamedCommands.registerCommand("RetourMilieuGauche", new RetourMilieuGauche());
     NamedCommands.registerCommand("Limelighter", new Limelighter(limeLight3G,drivetrain));
@@ -87,6 +85,12 @@ public class RobotContainer {
     NamedCommands.registerCommand("TournerA180", new TournerA180(drivetrain));
     NamedCommands.registerCommand("MonterGrimpeur", new MonterGrimpeur(grimpeur));
     NamedCommands.registerCommand("DescendreGrimpeur", new DescendreGrimpeur(grimpeur));
+    autoChooser = AutoBuilder.buildAutoChooser();
+    SmartDashboard.putData("Auto Chooser", autoChooser);
+
+    CameraServer.startAutomaticCapture();
+    
+    configureBindings();
   }
 
   private void configureBindings() {
@@ -127,5 +131,6 @@ public class RobotContainer {
 
   public Command getAutonomousCommand() {
     return autoChooser.getSelected();
+    // return getAutonomousCommand();
   }
 }

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

@@ -41,6 +41,6 @@ public class DescendreBalyeuse extends Command {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return Math.abs(balayeuse.Distance()) > balayeuse.EncodeurBalayeuse();
   }
 }

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

@@ -0,0 +1,42 @@
+// 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.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Led;
+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 DitanceLancer extends Command {
+  private Led led;
+  private Limelight3G limelight3g;
+  /** Creates a new DitanceLancer. */
+  public DitanceLancer(Limelight3G limelight3g, Led led) {
+    this.led = led;
+    this.limelight3g = limelight3g;
+    addRequirements(led,limelight3g);
+    // 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() {
+    
+  }
+
+  // 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;
+  }
+}

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

@@ -25,6 +25,7 @@ public class Lancer extends Command {
     this.timer = new Timer();
     this.limeLight3G = new Limelight3G();
     addRequirements(lanceur, limeLight3G);
+
     this.temp = 0;
     // Use addRequirements() here to declare subsystem dependencies.
   }
@@ -48,10 +49,10 @@ public class Lancer extends Command {
       botx = BotPose[0];
       boty = BotPose[1];
     }                                        
-         double vitesse = 0.5;
+    double vitesse = 0.5;
-         if(limeLight3G.getV()){
+    if(limeLight3G.getV()){
-            vitesse = 3100 + 475 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2))) - 2);
+      vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250;
-            System.out.println(vitesse);
+      System.out.println(vitesse);
 
       double output = pidController.calculate(lanceur.Vitesse(),vitesse);
       lanceur.Lancer(output);

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

@@ -24,6 +24,7 @@ public class Limelighter extends Command {
   double botx;
   double boty;
   double angle;
+  double calcul;
   private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
       .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
       .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
@@ -55,7 +56,7 @@ public class Limelighter extends Command {
       if(angle >180){
         angle =- 360;
       }
-      double calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 20;  
+      calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 5;  
       drivetrain.setControl(
           drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
       System.out.println(calcul);

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

@@ -4,35 +4,59 @@
 
 package frc.robot.commands.ModeAuto;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Lanceur;
+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 LancerAuto extends Command {
   Lanceur lanceur;
   Timer timer;
+  private PIDController pidController;
+  Limelight3G limelight3g;
+  double botx = 0;
+  double boty = 0;
   /** Creates a new LancerAuto. */
-  public LancerAuto(Lanceur lanceur) {
+  public LancerAuto(Lanceur lanceur, Limelight3G limelight3g) {
     this.lanceur = lanceur;
     timer = new Timer();
-    addRequirements(lanceur);
+    this.limelight3g = limelight3g;
+    addRequirements(lanceur, limelight3g);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    timer.start();
+    pidController = new PIDController(0.0007, 0,0, 0.001);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    lanceur.Lancer(0.5);
+    double[] BotPose = new double[6];
-      if(timer.get() > 1){
+    if(limelight3g.getV()){
-        lanceur.Demeler(0.1);
+      BotPose = limelight3g.getBotPoseBlue();
-     }
+      botx = BotPose[0];
+      boty = BotPose[1];
+    }                                        
+    double vitesse = 0.5;
+    if(limelight3g.getV()){
+      vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250;
+      System.out.println(vitesse);
+
+      double output = pidController.calculate(lanceur.Vitesse(),vitesse);
+      lanceur.Lancer(output);
+      if(lanceur.Vitesse() >= vitesse-800){
+        timer.start();
+        if(timer.get() >0.5){
+          lanceur.Demeler(1);
+        }
+
+    }
+  }
   }
 
   // Called once the command ends or is interrupted.
@@ -47,7 +71,7 @@ public class LancerAuto extends Command {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return timer.get() > 3;
+    return timer.get() > 4;
     //return false;
   }
 }

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

@@ -0,0 +1,91 @@
+// 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.commands.ModeAuto;
+
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.Limelight3G;
+import static edu.wpi.first.units.Units.*;
+
+/* 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 LimelighterAuto extends Command {
+  Timer timer;
+  Limelight3G limelight3g;
+  CommandSwerveDrivetrain drivetrain;
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
+  double botx;
+  double boty;
+  double angle;
+  double calcul;
+  private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+      .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
+      .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
+
+  /** Creates a new Limelighter. */
+  public LimelighterAuto(Limelight3G limelight3g, CommandSwerveDrivetrain drivetrain) {
+    this.limelight3g = limelight3g;
+    this.drivetrain = drivetrain;
+    addRequirements(drivetrain, limelight3g);
+    timer = new Timer();
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    timer.reset();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    double[] BotPose = new double[6];
+    if (limelight3g.getV()) {
+      BotPose = limelight3g.getBotPoseBlue();
+      botx = BotPose[1];
+      boty = BotPose[0];
+      angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
+      if(angle >180){
+        angle =- 360;
+      }
+      calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 5;  
+      drivetrain.setControl(
+          drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
+      System.out.println(calcul);
+      if (calcul < 0.2 && calcul > -0.2) {
+    //     timer.start();
+    //   } else {
+    //     timer.reset();
+    //   }
+    // } else {
+    //   timer.stop();
+      drivetrain.setControl(drive.withRotationalRate(0));
+      }
+    }
+    else{
+      drivetrain.setControl(drive.withRotationalRate(0));
+    }
+  }
+
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    drivetrain.setControl(drive.withRotationalRate(0));
+    timer.stop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return calcul < 0.1 && calcul > -0.1;
+  }
+}

src/main/java/frc/robot/subsystems/Led.java

@@ -4,11 +4,14 @@
 
 package frc.robot.subsystems;
 
+import java.util.Optional;
+
 import com.ctre.phoenix.led.CANdle;
 import com.ctre.phoenix.led.RainbowAnimation;
 
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;