From e42464b1d688bbea74e016135c4818b9b1a7343c Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Tue, 10 Mar 2026 20:38:21 -0400 Subject: [PATCH] Commandes auto sauf 2 --- src/main/java/frc/robot/RobotContainer.java | 6 +-- src/main/java/frc/robot/commands/Lancer.java | 1 - .../java/frc/robot/commands/Limelighter.java | 46 ++++++++++++++++-- .../robot/commands/ModeAuto/GrimperMur.java | 45 ++++++++++++++++-- .../commands/ModeAuto/GrimperReservoir.java | 47 +++++++++++++++++-- .../robot/commands/ModeAuto/LancerAuto.java | 5 ++ 6 files changed, 133 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cd1656d..3159c0b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -71,12 +71,12 @@ public class RobotContainer { autoChooser = AutoBuilder.buildAutoChooser(); CameraServer.startAutomaticCapture(); configureBindings(); - NamedCommands.registerCommand("GrimperMur", new GrimperMur()); - NamedCommands.registerCommand("GrimperReservoir", new GrimperReservoir()); + NamedCommands.registerCommand("GrimperMur", new GrimperMur(limeLight3,drivetrain)); + NamedCommands.registerCommand("GrimperReservoir", new GrimperReservoir(limeLight3G,drivetrain)); NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur)); NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite()); NamedCommands.registerCommand("RetourMilieuGauche", new RetourMilieuGauche()); - NamedCommands.registerCommand("Limelighter", new Limelighter()); + 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)); diff --git a/src/main/java/frc/robot/commands/Lancer.java b/src/main/java/frc/robot/commands/Lancer.java index 5a7674b..1ff9b42 100644 --- a/src/main/java/frc/robot/commands/Lancer.java +++ b/src/main/java/frc/robot/commands/Lancer.java @@ -10,7 +10,6 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Balayeuse; import frc.robot.subsystems.Lanceur; import frc.robot.subsystems.Led; -import frc.robot.subsystems.LimeLight3; 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 */ diff --git a/src/main/java/frc/robot/commands/Limelighter.java b/src/main/java/frc/robot/commands/Limelighter.java index 1f409ef..dde9b48 100644 --- a/src/main/java/frc/robot/commands/Limelighter.java +++ b/src/main/java/frc/robot/commands/Limelighter.java @@ -4,12 +4,34 @@ package frc.robot.commands; +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 Limelighter 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; + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); /** Creates a new Limelighter. */ - public Limelighter() { + public Limelighter(Limelight3G limelight3g,CommandSwerveDrivetrain drivetrain) { + this.limelight3g = limelight3g; + this.drivetrain = drivetrain; + addRequirements(drivetrain,limelight3g); // Use addRequirements() here to declare subsystem dependencies. } @@ -19,15 +41,31 @@ public class Limelighter extends Command { // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + double[] BotPose = new double[6]; + BotPose = limelight3g.getBotPoseBlue(); + botx = BotPose[0]; + boty = BotPose[1]; + angle = BotPose[5]; + drivetrain.setControl(drive.withRotationalRate(limelight3g.Calcule(4.625594, botx, 4.034536, boty, BotPose[5])/90)); + if(limelight3g.Calcule(4.625594, botx, 4.034536, boty, angle)/90 < 0.1){ + timer.start(); + } + else{ + timer.reset(); + } + } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + drivetrain.setControl(drive.withRotationalRate(0)); + timer.stop(); + } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return timer.get() > 1; } } diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java index 88da477..3ebd3d5 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java @@ -4,12 +4,37 @@ package frc.robot.commands.ModeAuto; +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 com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; + import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.generated.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 GrimperMur extends Command { + LimeLight3 limeLight3; + CommandSwerveDrivetrain drivetrain; + double[] BotPose = new double[6]; + double botx; + double boty; + 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 GrimperMur. */ - public GrimperMur() { + public GrimperMur(LimeLight3 limeLight3, CommandSwerveDrivetrain drivetrain) { + this.drivetrain = drivetrain; + this.limeLight3 = limeLight3; + addRequirements(limeLight3,drivetrain); // Use addRequirements() here to declare subsystem dependencies. } @@ -19,15 +44,27 @@ public class GrimperMur extends Command { // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + BotPose = limeLight3.getBotPoseBlue(); + botx = BotPose[0]; + boty = BotPose[1]; + 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(180-pigeon2.getYaw().getValueAsDouble()/45)); + } + } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0)); + } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + 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 571c454..ccd16ea 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java @@ -4,12 +4,37 @@ package frc.robot.commands.ModeAuto; +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 com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; + import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.generated.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 GrimperReservoir extends Command { - /** Creates a new GrimperReservoir. */ - public GrimperReservoir() { + Limelight3G limeLight3G; + CommandSwerveDrivetrain drivetrain; + double[] BotPose = new double[6]; + double botx; + double boty; + 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 GrimperMur. */ + public GrimperReservoir(Limelight3G limeLight3G, CommandSwerveDrivetrain drivetrain) { + this.drivetrain = drivetrain; + this.limeLight3G = limeLight3G; + addRequirements(limeLight3G,drivetrain); // Use addRequirements() here to declare subsystem dependencies. } @@ -19,15 +44,27 @@ public class GrimperReservoir extends Command { // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + BotPose = limeLight3G.getBotPoseBlue(); + botx = BotPose[0]; + boty = BotPose[1]; + 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(0-pigeon2.getYaw().getValueAsDouble()/45)); + } + } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0)); + } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + 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/LancerAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java index ee0ccb1..749233a 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java @@ -15,6 +15,7 @@ public class LancerAuto extends Command { /** Creates a new LancerAuto. */ public LancerAuto(Lanceur lanceur) { this.lanceur = lanceur; + addRequirements(lanceur); // Use addRequirements() here to declare subsystem dependencies. } @@ -28,12 +29,16 @@ public class LancerAuto extends Command { @Override public void execute() { lanceur.Lancer(0.5); + if(timer.get() > 1){ + lanceur.Demeler(0.5); + } } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { lanceur.Lancer(0); + lanceur.Demeler(0); timer.reset(); timer.stop(); }