limelight

simgui-ds.json

@@ -1,4 +1,9 @@
 {
+  "System Joysticks": {
+    "window": {
+      "enabled": false
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [

src/main/deploy/pathplanner/autos/BlueBas3 sans composantes.auto

@@ -0,0 +1,126 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueBasStart"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueBasChercher"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueBasPorter"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueBasChercher2"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": []
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueBasPorter2"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/New Auto.auto

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "New Path"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -4,6 +4,12 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 3.0
+          }
+        },
         {
           "type": "path",
           "data": {

src/main/deploy/pathplanner/paths/New Path.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.8820696721311476,
+        "y": 6.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.8706512438743716,
+        "y": 5.690872003047288
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.8820696721311476,
+        "y": 3.1279999999999997
+      },
+      "prevControl": {
+        "x": 1.8863317961339294,
+        "y": 3.3779636659576444
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Sortir.path

@@ -8,20 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 7.084733606557376,
+        "x": 7.119564732833961,
-        "y": 6.488473360655737
+        "y": 6.478684258786044
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.849999999999999,
+        "x": 5.838012295081967,
-        "y": 6.464497950819672
+        "y": 7.189754098360656
       },
       "prevControl": {
-        "x": 6.32950819672131,
+        "x": 6.656071206876743,
-        "y": 6.464497950819672
+        "y": 7.180867730637985
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,10 +33,10 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 0.1,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 0.1,
-    "maxAngularVelocity": 540.0,
+    "maxAngularVelocity": 50.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 100.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 180.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/settings.json

@@ -14,7 +14,7 @@
   "robotTrackwidth": 0.546,
   "driveWheelRadius": 0.048,
   "driveGearing": 5.143,
-  "maxDriveSpeed": 5.45,
+  "maxDriveSpeed": 5.0,
   "driveMotorType": "krakenX60",
   "driveCurrentLimit": 60.0,
   "wheelCOF": 1.2,

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

@@ -7,6 +7,7 @@ package frc.robot;
 import static edu.wpi.first.units.Units.*;
 
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.hardware.Pigeon2;
 import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest.PointWheelsAt;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
@@ -15,12 +16,15 @@ import com.pathplanner.lib.auto.NamedCommands;
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 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.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 
@@ -56,7 +60,12 @@ public class RobotContainer {
     Bougie bougie = new Bougie();
     Limelight3G limelight3g = new Limelight3G();
     Limelight3 limelight3 = new Limelight3();
-    public RobotContainer() {
+    Pose2d pose = new Pose2d();
+    private final Pigeon2 gyro = new Pigeon2(13); // ID du Pigeon 2
+    public double getAngle() {
+        return gyro.getYaw().getValueAsDouble(); // Retourne l'angle actuel du robot
+    }
+        public RobotContainer() {
         autoChooser = AutoBuilder.buildAutoChooser("New Auto");
         SmartDashboard.putData("Auto Mode", autoChooser);
         configureBindings();
@@ -68,25 +77,25 @@ public class RobotContainer {
         drivetrain.setDefaultCommand(
             // Drivetrain will execute this command periodically
             drivetrain.applyRequest(() ->
-                drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY()*MaxSpeed, 0.15)) // Drive forward with negative Y (forward)
+                drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY()*MaxSpeed, 0.2)) // Drive forward with negative Y (forward)
-                    .withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX()*MaxSpeed, 0.15)) // Drive left with negative X (left)
+                    .withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX()*MaxSpeed, 0.2)) // Drive left with negative X (left)
-                    .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.1)) // Drive counterclockwise with negative X (left)
+                    .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.15)) // Drive counterclockwise with negative X (left)
             )
         );
 
 
         // reset the field-centric heading on left bumper press
         manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
-        manette1.leftTrigger().whileTrue(new AprilTag3(limelight3,drivetrain));
+        manette1.leftTrigger().whileTrue(new AprilTag3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
-        manette1.rightTrigger().whileTrue(new Forme3(limelight3));
+        manette1.rightTrigger().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
-        manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g));
+        manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY));
         drivetrain.registerTelemetry(logger::telemeterize);
     }
 
    
         public Command getAutonomousCommand() {
-            return new ParallelCommandGroup(
+            return new SequentialCommandGroup(autoChooser.getSelected(),
-              autoChooser.getSelected(),
+             new RainBow(bougie));
-            new RainBow(bougie));
+    }
-         }
+             
 }

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

@@ -5,6 +5,8 @@
 package frc.robot.commands;
 import static edu.wpi.first.units.Units.*;
 
+import java.util.function.DoubleSupplier;
+
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
@@ -18,16 +20,19 @@ public class AprilTag3 extends Command {
   private Limelight3 limelight3;
   private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
   private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
-    private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
-
+  private DoubleSupplier x;
+  private DoubleSupplier y;
     /* Setting up bindings for necessary control of the swerve drive platform */
     private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
             .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
             .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
   /** Creates a new AprilTag3G. */
-  public AprilTag3(Limelight3 limelight3,CommandSwerveDrivetrain drivetrain) {
+  public AprilTag3(Limelight3 limelight3,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) {
     this.limelight3 = limelight3;
     this.drivetrain = drivetrain;
+    this.x = x;
+    this.y = y;
     addRequirements(limelight3,drivetrain);
     
     // Use addRequirements() here to declare subsystem dependencies.
@@ -42,24 +47,30 @@ public class AprilTag3 extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    double a = limelight3.getX();
    
     if(limelight3.getV() == true){
-      drivetrain.applyRequest(() ->
+      drivetrain.setControl(drive.
-      drive.withRotationalRate(limelight3.getX()/48));  
+      withRotationalRate(a/10).
-       System.out.println(limelight3.getX()/48);
+      withVelocityX(x.getAsDouble()).
+      withVelocityY(y.getAsDouble()));  
+       System.out.println(a/10);
     }
     else{
-      drivetrain.applyRequest(() ->
+      drivetrain.setControl(drive.
-      drive.withRotationalRate(0)
+      withRotationalRate(0).
-      );
+      withVelocityX(0).
+      withVelocityY(0));
     }
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    drivetrain.applyRequest(() ->
+    drivetrain.setControl(drive.
-    drive.withRotationalRate(0));
+    withRotationalRate(0)
+    .withVelocityX(0)
+    .withVelocityY(0));
   }
 
   // Returns true when the command should end.

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

@@ -4,29 +4,37 @@
 
 package frc.robot.commands;
 import static edu.wpi.first.units.Units.*;
+
+import java.util.function.DoubleSupplier;
+
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.TunerConstants.TunerConstants;
-
+import frc.robot.subsystems.CommandSwerveDrivetrain;
 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 AprilTag3G extends Command {
   private Limelight3G limelight3g;
+  private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
   private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
-    private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
-
+  private DoubleSupplier x;
+  private DoubleSupplier y;
     /* Setting up bindings for necessary control of the swerve drive platform */
     private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
             .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
             .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
   /** Creates a new AprilTag3G. */
   /** Creates a new AprilTag3G. */
-  public AprilTag3G(Limelight3G limelight3g) {
+  public AprilTag3G(Limelight3G limelight3g,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) {
     this.limelight3g = limelight3g;
-    addRequirements(limelight3g);
+    this.drivetrain = drivetrain;
+    this.x = x;
+    this.y = y;
+    addRequirements(limelight3g,drivetrain);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -37,18 +45,29 @@ public class AprilTag3G extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    double a = limelight3g.getX();
     if(limelight3g.getV() == true){
-      drive.withRotationalRate(limelight3g.getX()/48);
+      drivetrain.setControl(drive.
+      withRotationalRate(-a/5).
+      withVelocityX(x.getAsDouble()).
+      withVelocityY(y.getAsDouble()));  
+       System.out.println(a/5);
     }
     else{
-      drive.withRotationalRate(0);
+      drivetrain.setControl(drive.
+      withRotationalRate(0).
+      withVelocityX(0).
+      withVelocityY(0));
     }
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    drive.withRotationalRate(0);
+    drivetrain.setControl(drive.
+      withRotationalRate(0).
+      withVelocityX(0).
+      withVelocityY(0));
   }
 
   // Returns true when the command should end.

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

@@ -4,28 +4,36 @@
 
 package frc.robot.commands;
 import static edu.wpi.first.units.Units.*;
+
+import java.util.function.DoubleSupplier;
+
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.TunerConstants.TunerConstants;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
 import frc.robot.subsystems.Limelight3;
 
 /* 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 Forme3 extends Command {
   private Limelight3 limelight3;
   private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
-    private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
-
+  private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
+  private DoubleSupplier x;
+  private DoubleSupplier y;
     /* Setting up bindings for necessary control of the swerve drive platform */
     private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
             .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
             .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
   /** Creates a new Forme3. */
-  public Forme3(Limelight3 limelight3) {
+  public Forme3(Limelight3 limelight3,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) {
     this.limelight3 = limelight3;
-    
+    this.drivetrain = drivetrain;
-    addRequirements(limelight3);
+    this.x = x;
+    this.y = y;
+    addRequirements(limelight3,drivetrain);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -38,18 +46,29 @@ public class Forme3 extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    double a = limelight3.getX();
     if(limelight3.getV() == true){
-      drive.withRotationalRate(limelight3.getX()/48);
+      drivetrain.setControl(drive.
+      withRotationalRate(a/10).
+      withVelocityX(x.getAsDouble()).
+      withVelocityY(y.getAsDouble()));  
+       System.out.println(a/10);
     }
     else{
-      drive.withRotationalRate(0);
+      drivetrain.setControl(drive.
+      withRotationalRate(0).
+      withVelocityX(0).
+      withVelocityY(0));
     }
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    drive.withRotationalRate(0);
+    drivetrain.setControl(drive.
+      withRotationalRate(0).
+      withVelocityX(0).
+      withVelocityY(0));
   }
 
   // Returns true when the command should end.

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

@@ -6,6 +6,7 @@ import java.util.function.Supplier;
 
 import com.ctre.phoenix6.SignalLogger;
 import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.hardware.Pigeon2;
 import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
 import com.ctre.phoenix6.swerve.SwerveModuleConstants;
 import com.ctre.phoenix6.swerve.SwerveRequest;
@@ -53,6 +54,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
 
     /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
+
     private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine(
         new SysIdRoutine.Config(
             null,        // Use default ramp rate (1 V/s)
@@ -255,7 +257,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     public Command sysIdDynamic(SysIdRoutine.Direction direction) {
         return m_sysIdRoutineToApply.dynamic(direction);
     }
-
+    
     @Override
     public void periodic() {
         /*