From 33fd6ceb230dd994e273dd75542e58580bb88e83 Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Tue, 24 Mar 2026 19:59:14 -0400 Subject: [PATCH 1/2] =?UTF-8?q?tourner=20selon=20l'=C3=A9quipe?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- simgui-ds.json | 5 -- src/main/deploy/pathplanner/settings.json | 22 +++--- src/main/java/frc/robot/RobotContainer.java | 7 +- .../robot/commands/ModeAuto/GrimperMur.java | 28 ++------ .../commands/ModeAuto/GrimperReservoir.java | 28 ++------ .../commands/ModeAuto/TournerVersMur.java | 68 +++++++++++++++++++ 6 files changed, 91 insertions(+), 67 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java diff --git a/simgui-ds.json b/simgui-ds.json index 5628a9e..c4b7efd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,9 +1,4 @@ { - "System Joysticks": { - "window": { - "enabled": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index f8995ab..dcaad73 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -16,19 +16,19 @@ "robotMass": 51.673, "robotMOI": 6.883, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.051, - "driveGearing": 6.122448979591837, - "maxDriveSpeed": 9.82, + "driveWheelRadius": 0.048, + "driveGearing": 5.143, + "maxDriveSpeed": 5.374, "driveMotorType": "krakenX60", - "driveCurrentLimit": 120.0, - "wheelCOF": 1.1, - "flModuleX": 0.289, - "flModuleY": 0.27, - "frModuleX": 0.289, + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2, + "flModuleX": 0.288925, + "flModuleY": 0.269875, + "frModuleX": 0.288925, "frModuleY": -0.27, - "blModuleX": -0.289, - "blModuleY": 0.27, - "brModuleX": -0.289, + "blModuleX": -0.288925, + "blModuleY": 0.269875, + "brModuleX": -0.288925, "brModuleY": -0.27, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2baa8ef..93a01f5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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.TournerAZero; +import frc.robot.commands.ModeAuto.TournerVersMur; 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 TournerA180(drivetrain)); + NamedCommands.registerCommand("TournerA180", new TournerVersMur(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)); diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java index b2a1686..3ebd3d5 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java +++ b/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 = DriverStation.getAlliance(); + Pigeon2 pigeon2; 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){ - drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); + if(180-pigeon2.getYaw().getValueAsDouble() < 10 && 180- pigeon2.getYaw().getValueAsDouble() > -10){ + 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); } } diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java index 3f26544..ccd16ea 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java +++ b/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 = DriverStation.getAlliance(); + Pigeon2 pigeon2; 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){ - drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); + if(0-pigeon2.getYaw().getValueAsDouble() < 10 && 0- pigeon2.getYaw().getValueAsDouble() > -10){ + 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; } } diff --git a/src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java b/src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java new file mode 100644 index 0000000..8aee71a --- /dev/null +++ b/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 = 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; + } +} From b83e689b977647cb3af9d261ae778fe7bb8842a9 Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Tue, 24 Mar 2026 20:00:16 -0400 Subject: [PATCH 2/2] enlever les deux tourner --- .../robot/commands/ModeAuto/TournerA180.java | 56 ------------------- .../robot/commands/ModeAuto/TournerAZero.java | 56 ------------------- 2 files changed, 112 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/ModeAuto/TournerA180.java delete mode 100644 src/main/java/frc/robot/commands/ModeAuto/TournerAZero.java diff --git a/src/main/java/frc/robot/commands/ModeAuto/TournerA180.java b/src/main/java/frc/robot/commands/ModeAuto/TournerA180.java deleted file mode 100644 index b19d21f..0000000 --- a/src/main/java/frc/robot/commands/ModeAuto/TournerA180.java +++ /dev/null @@ -1,56 +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.ModeAuto; - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.CommandSwerveDrivetrain; -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 TournerA180 extends Command { - CommandSwerveDrivetrain drivetrain; - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); - Pigeon2 pigeon2; - private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - /** Creates a new TournerAZero. */ - public TournerA180(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(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(-0.5)); - } - else if(pigeon2.getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(0.5)); - } - } - - // 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; - } -} diff --git a/src/main/java/frc/robot/commands/ModeAuto/TournerAZero.java b/src/main/java/frc/robot/commands/ModeAuto/TournerAZero.java deleted file mode 100644 index 9e45d08..0000000 --- a/src/main/java/frc/robot/commands/ModeAuto/TournerAZero.java +++ /dev/null @@ -1,56 +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.ModeAuto; - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.CommandSwerveDrivetrain; -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 TournerAZero extends Command { - CommandSwerveDrivetrain drivetrain; - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); - Pigeon2 pigeon2; - private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - /** Creates a new TournerAZero. */ - public TournerAZero(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(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(0.5)); - } - else if(pigeon2.getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(-0.5)); - } - } - - // 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() > 345 || pigeon2.getYaw().getValueAsDouble() < 15; - } -}