debug de jeudi

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

@@ -33,7 +33,7 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 5.0,
     "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 0.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/settings.json

@@ -16,12 +16,12 @@
   "robotMass": 51.673,
   "robotMOI": 6.883,
   "robotTrackwidth": 0.546,
-  "driveWheelRadius": 0.048,
+  "driveWheelRadius": 0.099,
-  "driveGearing": 5.143,
+  "driveGearing": 6.2,
-  "maxDriveSpeed": 5.374,
+  "maxDriveSpeed": 9.82,
   "driveMotorType": "krakenX60",
-  "driveCurrentLimit": 60.0,
+  "driveCurrentLimit": 120.0,
-  "wheelCOF": 1.2,
+  "wheelCOF": 1.1,
   "flModuleX": 0.288925,
   "flModuleY": 0.269875,
   "frModuleX": 0.288925,

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

@@ -20,7 +20,6 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.Aspirer;
 import frc.robot.commands.DescendreBalyeuse;
 import frc.robot.commands.DescendreGrimpeur;
-import frc.robot.commands.DistanceLancer;
 import frc.robot.commands.Lancer;
 import frc.robot.commands.LancerAspirer;
 import frc.robot.commands.LancerBaseVitesse;
@@ -94,7 +93,6 @@ public class RobotContainer {
   }
 
   private void configureBindings() {
-    led.setDefaultCommand(new DistanceLancer(limeLight3G, led));
     drivetrain.setDefaultCommand(
           drivetrain.applyRequest(() ->
             drive.withVelocityY(MathUtil.applyDeadband(-manette.getLeftX()*Math.abs(-manette.getLeftX())*MaxSpeed, 0.05)) 

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

@@ -1,61 +0,0 @@
-// 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 DistanceLancer extends Command {
-  private Led led;
-  private Limelight3G limelight3g;
-  /** Creates a new DitanceLancer. */
-  public DistanceLancer(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() {
-    double botx = 0;
-    double boty = 0;
-    double[] BotPose = new double[6];
-    double distance = 0;
-    if(limelight3g.getV()){
-      BotPose = limelight3g.getBotPoseBlue();
-      botx = BotPose[0];
-      boty = BotPose[1];
-    }                                        
-    if(limelight3g.getV()){
-      distance = Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2));
-    }
-    if(distance > 1.7){
-      led.Jaune2();
-    }
-    else{
-      
-    }
-  }
-
-  // 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

@@ -20,7 +20,7 @@ public class Lancer extends Command {
   public Lancer(Lanceur lanceur, Limelight3G limeLight3G) {
     this.lanceur = lanceur;
     this.timer = new Timer();
-    this.limeLight3G = new Limelight3G();
+    this.limeLight3G = limeLight3G;
     addRequirements(lanceur, limeLight3G);
     // Use addRequirements() here to declare subsystem dependencies.
   }

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

@@ -61,12 +61,12 @@ public class Limelighter extends Command {
       boty = BotPose[0];
       angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
       if(angle >180){
-        angle =- 360;
+        angle -= 360;
       }
-      calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 5;  
+      calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 10;  
       drivetrain.setControl(
           drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
-      System.out.println(calcul);
+      System.out.println(angle);
       if (calcul < 0.2 && calcul > -0.2) {
       drivetrain.setControl(drive.withRotationalRate(0));
       }

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

@@ -7,16 +7,18 @@ 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.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 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.*;
 
+import java.util.Optional;
+
 /* 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);
@@ -25,6 +27,7 @@ public class LimelighterAuto extends Command {
   double boty;
   double angle;
   double calcul;
+   Optional<Alliance> alliance = DriverStation.getAlliance(); 
   private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
       .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
       .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
@@ -34,39 +37,37 @@ public class LimelighterAuto extends Command {
     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() {
+  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];
+    System.out.println("e");
     if (limelight3g.getV()) {
-      BotPose = limelight3g.getBotPoseBlue();
+      if(alliance.get() == Alliance.Blue){
+        BotPose = limelight3g.getBotPoseBlue();
+      }
+      else{
+        BotPose = limelight3g.getBotPoseRed();
+      }
+      
       botx = BotPose[1];
       boty = BotPose[0];
       angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
       if(angle >180){
-        angle =- 360;
+        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));
       }
     }
@@ -80,7 +81,6 @@ public class LimelighterAuto extends Command {
   @Override
   public void end(boolean interrupted) {
     drivetrain.setControl(drive.withRotationalRate(0));
-    timer.stop();
   }
 
   // Returns true when the command should end.
@@ -89,3 +89,4 @@ public class LimelighterAuto extends Command {
     return calcul < 0.1 && calcul > -0.1;
   }
 }
+

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

@@ -248,12 +248,12 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
 
     @Override
     public void periodic() {
-        // if(getPigeon2().getYaw().getValueAsDouble() > 360){
+        if(getPigeon2().getYaw().getValueAsDouble() > 360){
-        //     getPigeon2().setYaw(getPigeon2().getYaw().getValueAsDouble()-360);
+            getPigeon2().setYaw(getPigeon2().getYaw().getValueAsDouble()-360);
-        // }
+        }
-        // else if(getPigeon2().getYaw().getValueAsDouble() < 0){
+        else if(getPigeon2().getYaw().getValueAsDouble() < 0){
-        //     getPigeon2().setYaw(getPigeon2().getYaw().getValueAsDouble()+360);
+            getPigeon2().setYaw(getPigeon2().getYaw().getValueAsDouble()+360);
-        //  }
+         }
         
         /*
          * Periodically try to apply the operator perspective.