tourner selon l'équipe

simgui-ds.json

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

src/main/deploy/pathplanner/settings.json

@@ -16,19 +16,19 @@
   "robotMass": 51.673,
   "robotMOI": 6.883,
   "robotTrackwidth": 0.546,
-  "driveWheelRadius": 0.051,
+  "driveWheelRadius": 0.048,
-  "driveGearing": 6.122448979591837,
+  "driveGearing": 5.143,
-  "maxDriveSpeed": 9.82,
+  "maxDriveSpeed": 5.374,
   "driveMotorType": "krakenX60",
-  "driveCurrentLimit": 120.0,
+  "driveCurrentLimit": 60.0,
-  "wheelCOF": 1.1,
+  "wheelCOF": 1.2,
-  "flModuleX": 0.289,
+  "flModuleX": 0.288925,
-  "flModuleY": 0.27,
+  "flModuleY": 0.269875,
-  "frModuleX": 0.289,
+  "frModuleX": 0.288925,
   "frModuleY": -0.27,
-  "blModuleX": -0.289,
+  "blModuleX": -0.288925,
-  "blModuleY": 0.27,
+  "blModuleY": 0.269875,
-  "brModuleX": -0.289,
+  "brModuleX": -0.288925,
   "brModuleY": -0.27,
   "bumperOffsetX": 0.0,
   "bumperOffsetY": 0.0,

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

@@ -37,8 +37,7 @@ import frc.robot.commands.ModeAuto.GrimperReservoir;
 import frc.robot.commands.ModeAuto.LancerAuto;
 import frc.robot.commands.ModeAuto.RetourMilieuDroite;
 import frc.robot.commands.ModeAuto.RetourMilieuGauche;
-import frc.robot.commands.ModeAuto.TournerA180;
+import frc.robot.commands.ModeAuto.TournerVersMur;
-import frc.robot.commands.ModeAuto.TournerAZero;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.Balayeuse;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
@@ -82,8 +81,7 @@ public class RobotContainer {
     NamedCommands.registerCommand("Limelighter", new Limelighter(limeLight3G,drivetrain));
     NamedCommands.registerCommand("DescendreBalayeuse", new DescendreBalyeuse(balayeuse));
     NamedCommands.registerCommand("Aspirer", new AspirerAuto(balayeuse));
-    NamedCommands.registerCommand("TournerAZero", new TournerAZero(drivetrain));
+    NamedCommands.registerCommand("TournerA180", new TournerVersMur(drivetrain));
-    NamedCommands.registerCommand("TournerA180", new TournerA180(drivetrain));
     NamedCommands.registerCommand("MonterGrimpeur", new MonterGrimpeur(grimpeur));
     NamedCommands.registerCommand("DescendreGrimpeur", new DescendreGrimpeur(grimpeur));
     autoChooser = AutoBuilder.buildAutoChooser();
@@ -115,7 +113,6 @@ public class RobotContainer {
     manette.leftTrigger().whileTrue(new Limelighter(limeLight3G, drivetrain));
     manette.leftBumper().whileTrue(new DescendreBalyeuse(balayeuse));
     manette.rightBumper().whileTrue(new Aspirer(balayeuse));
-    manette.y().whileTrue(new TournerAZero(drivetrain));
     manette.b().whileTrue(new GrimperReservoir(limeLight3G, drivetrain));
     manette.x().whileTrue(new GrimperMur(limeLight3, drivetrain));
 

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

@@ -8,14 +8,10 @@ import static edu.wpi.first.units.Units.MetersPerSecond;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 
-import java.util.Optional;
-
 import com.ctre.phoenix6.hardware.Pigeon2;
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
-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;
@@ -28,13 +24,9 @@ public class GrimperMur extends Command {
   double[] BotPose = new double[6];
   double botx;
   double boty;
-  double x;
-  double y;
-  double angle;
   private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
   private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
   Pigeon2 pigeon2; 
-  Optional<Alliance> alliance = DriverStation.getAlliance();
   private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
             .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
             .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
@@ -53,24 +45,14 @@ public class GrimperMur extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(alliance.get() == Alliance.Blue){
-     y = 2.961328;
-     x = 1.11;
-     angle = 180;
-    }
-    else{
-     x = 11.45966;
-     y = 6.959326;
-     angle = 0;
-    }
     BotPose = limeLight3.getBotPoseBlue();
     botx = BotPose[0];
     boty = BotPose[1];
-    if(angle-pigeon2.getYaw().getValueAsDouble() < angle+10 && angle- pigeon2.getYaw().getValueAsDouble() > angle-10){
+    if(180-pigeon2.getYaw().getValueAsDouble() < 10 && 180- pigeon2.getYaw().getValueAsDouble() > -10){
-      drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx));
+      drivetrain.setControl(drive.withVelocityX(2.961328-boty).withVelocityY(1.11-botx));
     }
     else{
-       drivetrain.setControl(drive.withRotationalRate(angle-pigeon2.getYaw().getValueAsDouble()*Math.PI/180));
+       drivetrain.setControl(drive.withRotationalRate(180-pigeon2.getYaw().getValueAsDouble()/45));
     }
   }
 
@@ -83,6 +65,6 @@ public class GrimperMur extends Command {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return (y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05);
+    return (2.961328-boty < 0.05 || 2.961328-boty >-0.05) && (1.11-botx < 0.05 || 1.11-botx > -0.05);
   }
 }

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

@@ -8,14 +8,10 @@ import static edu.wpi.first.units.Units.MetersPerSecond;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 
-import java.util.Optional;
-
 import com.ctre.phoenix6.hardware.Pigeon2;
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
-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;
@@ -28,13 +24,9 @@ public class GrimperReservoir extends Command {
   double[] BotPose = new double[6];
   double botx;
   double boty;
-  double x;
-  double y;
-  double angle;
   private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
   private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
   Pigeon2 pigeon2; 
-    Optional<Alliance> alliance = DriverStation.getAlliance(); 
   private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
             .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
             .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
@@ -53,24 +45,14 @@ public class GrimperReservoir extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(alliance.get() == Alliance.Blue){
-     y = 5.081328;
-     x = 1.11;
-     angle = 0;
-    }
-    else{
-     y = 6.959326;
-     x = 13.57966;
-     angle = 180;
-    }
     BotPose = limeLight3G.getBotPoseBlue();
     botx = BotPose[0];
     boty = BotPose[1];
-    if(angle-pigeon2.getYaw().getValueAsDouble() < angle+10 && angle- pigeon2.getYaw().getValueAsDouble() > angle-10){
+    if(0-pigeon2.getYaw().getValueAsDouble() < 10 && 0- pigeon2.getYaw().getValueAsDouble() > -10){
-      drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx));
+      drivetrain.setControl(drive.withVelocityX(5.081328-boty).withVelocityY(1.11-botx));
     }
     else{
-       drivetrain.setControl(drive.withRotationalRate(angle-pigeon2.getYaw().getValueAsDouble()*Math.PI/180));
+       drivetrain.setControl(drive.withRotationalRate(0-pigeon2.getYaw().getValueAsDouble()/45));
     }
   }
 
@@ -83,6 +65,6 @@ public class GrimperReservoir extends Command {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return (y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05);
+    return 5.081328-boty < 0.05 || 5.081328-boty >-0.05 && 1.11-botx < 0.05 || 1.11-botx > -0.05;
   }
 }

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

@@ -0,0 +1,68 @@
+// 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.hardware.Pigeon2;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+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 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 TournerVersMur extends Command {
+  CommandSwerveDrivetrain drivetrain;
+  Optional<Alliance> alliance = DriverStation.getAlliance();
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
+  Pigeon2 pigeon2; 
+  double force;
+  private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
+  /** Creates a new TournerAZero. */
+  public TournerVersMur(CommandSwerveDrivetrain drivetrain) {
+    this.drivetrain = drivetrain;
+    addRequirements(drivetrain);
+    // 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() {
+    if(alliance.get() == Alliance.Blue){
+      force = 0.5;
+    }
+    else{
+      force = -0.5;
+    }
+    if(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){
+      drivetrain.setControl(drive.withRotationalRate(force));
+    }
+    else if(pigeon2.getYaw().getValueAsDouble() >180){
+      drivetrain.setControl(drive.withRotationalRate(-force));
+    }
+  }
+
+  // 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 pigeon2.getYaw().getValueAsDouble() > 165  && pigeon2.getYaw().getValueAsDouble() < 195;
+  }
+}