From d0c50cbd6e99ad07b7fe6028ac0b37ef2c124564 Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Thu, 26 Mar 2026 17:46:22 -0400 Subject: [PATCH] mode auto pls marche pls --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/commands/Limelighter.java | 13 ++- .../robot/commands/ModeAuto/GrimperMur.java | 48 +++++++- .../commands/ModeAuto/GrimperReservoir.java | 48 +++++++- .../commands/ModeAuto/RetourMilieuDroite.java | 81 ++++++++++--- .../commands/ModeAuto/RetourMilieuGauche.java | 110 +++++++++++++++++- .../commands/ModeAuto/TournerVersMur.java | 2 +- .../ModeAuto/TournerVersReservoir.java | 2 +- 8 files changed, 272 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1a5f24b..fa5c667 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,7 +78,7 @@ public class RobotContainer { NamedCommands.registerCommand("GrimperReservoir", new GrimperReservoir(limeLight3G,drivetrain)); NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur, limeLight3G)); NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite(drivetrain,limeLight3G)); - NamedCommands.registerCommand("RetourMilieuGauche", new RetourMilieuGauche()); + NamedCommands.registerCommand("RetourMilieuGauche", new RetourMilieuGauche(drivetrain, limeLight3G)); NamedCommands.registerCommand("Limelighter", new Limelighter(limeLight3G,drivetrain)); NamedCommands.registerCommand("DescendreBalayeuse", new DescendreBalyeuse(balayeuse)); NamedCommands.registerCommand("Aspirer", new AspirerAuto(balayeuse)); diff --git a/src/main/java/frc/robot/commands/Limelighter.java b/src/main/java/frc/robot/commands/Limelighter.java index b4255a2..f11559d 100644 --- a/src/main/java/frc/robot/commands/Limelighter.java +++ b/src/main/java/frc/robot/commands/Limelighter.java @@ -7,6 +7,8 @@ package frc.robot.commands; 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.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.generated.TunerConstants; @@ -14,6 +16,8 @@ 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 Limelighter extends Command { Timer timer; @@ -25,6 +29,7 @@ public class Limelighter extends Command { double boty; double angle; double calcul; + Optional alliance = DriverStation.getAlliance(); private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) .withDriveRequestType(DriveRequestType.OpenLoopVoltage); @@ -49,7 +54,13 @@ public class Limelighter extends Command { public void execute() { double[] BotPose = new double[6]; 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(); diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java index 3ebd3d5..580da41 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java @@ -8,10 +8,14 @@ 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; @@ -24,9 +28,13 @@ 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(); private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) .withDriveRequestType(DriveRequestType.OpenLoopVoltage); @@ -48,12 +56,42 @@ public class GrimperMur extends Command { 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)); + if(angle < 0){ + angle = angle + 360; + } + if(alliance.get() == Alliance.Blue){ + y = 2.961328; + x = 1.11; + angle = 0; + if(pigeon2.getYaw().getValueAsDouble() > 355 || pigeon2.getYaw().getValueAsDouble() < 5){ + drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); } else{ - drivetrain.setControl(drive.withRotationalRate(180-pigeon2.getYaw().getValueAsDouble()/45)); + 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)); + } } + } + else{ + x = 11.45966; + y = 6.959326; + angle = 180; + if(pigeon2.getYaw().getValueAsDouble() > 175 && pigeon2.getYaw().getValueAsDouble() < 185){ + drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); + } + else{ + 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. @@ -65,6 +103,6 @@ public class GrimperMur extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return (2.961328-boty < 0.05 || 2.961328-boty >-0.05) && (1.11-botx < 0.05 || 1.11-botx > -0.05); + return (y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java index ccd16ea..02659db 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java @@ -8,10 +8,14 @@ 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; @@ -24,9 +28,13 @@ 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(); private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) .withDriveRequestType(DriveRequestType.OpenLoopVoltage); @@ -48,12 +56,42 @@ public class GrimperReservoir extends Command { 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)); + if(angle < 0){ + angle = angle + 360; + } + if(alliance.get() == Alliance.Blue){ + y = 5.081328; + x = 1.11; + angle = 180; + if(pigeon2.getYaw().getValueAsDouble() < 185 && pigeon2.getYaw().getValueAsDouble() > 175){ + drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); } else{ - drivetrain.setControl(drive.withRotationalRate(0-pigeon2.getYaw().getValueAsDouble()/45)); + 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)); + } } + } + else{ + y = 6.959326; + x = 13.57966; + angle = 0; + if(pigeon2.getYaw().getValueAsDouble() > 355 || pigeon2.getYaw().getValueAsDouble() < 5){ + drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); + } + else{ + 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. @@ -65,6 +103,6 @@ public class GrimperReservoir extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return 5.081328-boty < 0.05 || 5.081328-boty >-0.05 && 1.11-botx < 0.05 || 1.11-botx > -0.05; + return (y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java b/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java index 1294f7d..2f70b54 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java +++ b/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java @@ -28,6 +28,7 @@ public class RetourMilieuDroite extends Command { double x; double y; double angle; + double force; private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); Pigeon2 pigeon2; @@ -50,36 +51,86 @@ public class RetourMilieuDroite extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + if(angle < 0){ + angle = angle + 360; + } double[] BotPose = new double[6]; BotPose = limelight3g.getBotPoseBlue(); botx = BotPose[0]; boty = BotPose[1]; if(alliance.get() == Alliance.Blue){ + y = 0.639; + x = 2.305; angle = 0; - } - else{ + force = 0.5; + if(limelight3g.getV()){ + if(pigeon2.getYaw().getValueAsDouble() >355 || pigeon2.getYaw().getValueAsDouble() < 5){ + if((y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05)){ + drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0)); + } + else{ + if(botx < 6){ + drivetrain.setControl(drive.withVelocityX(y-boty)); + } + else{ + drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); + } + } + } + else{ + if(pigeon2.getYaw().getValueAsDouble() >angle && pigeon2.getYaw().getValueAsDouble() =angle +180){ + + drivetrain.setControl(drive.withRotationalRate(-0.5)); + } + } + } + } + else{ + y = 7.380; + x = 13.963; angle = 180; - } - if(pigeon2.getYaw().getValueAsDouble() <355 || pigeon2.getYaw().getValueAsDouble() > 5){ - 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)); - } - } - else{ - } + force = -0.5; + if(limelight3g.getV()){ + if(pigeon2.getYaw().getValueAsDouble() >175 && pigeon2.getYaw().getValueAsDouble() < 185){ + if((y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05)){ + drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0)); + } + else{ + if(botx > 10){ + drivetrain.setControl(drive.withVelocityX(y-boty)); + } + else{ + drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); + } + } + } + else{ + 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) {} + public void end(boolean interrupted) { + drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0)); + } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return (y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05); } } diff --git a/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuGauche.java b/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuGauche.java index 50d606b..5f430cf 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuGauche.java +++ b/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuGauche.java @@ -4,12 +4,42 @@ package frc.robot.commands.ModeAuto; +import static edu.wpi.first.units.Units.*; + +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; +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 RetourMilieuGauche extends Command { - /** Creates a new RetourMilieuGauche. */ - public RetourMilieuGauche() { + CommandSwerveDrivetrain drivetrain; + Limelight3G limelight3g; + 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(); + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + /** Creates a new RetourMilieu. */ + public RetourMilieuGauche(CommandSwerveDrivetrain drivetrain, Limelight3G limelight3g) { + this.drivetrain = drivetrain; + this.limelight3g = limelight3g; + addRequirements(drivetrain, limelight3g); // Use addRequirements() here to declare subsystem dependencies. } @@ -19,15 +49,85 @@ public class RetourMilieuGauche extends Command { // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + if(angle < 0){ + angle = angle + 360; + } + double[] BotPose = new double[6]; + BotPose = limelight3g.getBotPoseBlue(); + botx = BotPose[0]; + boty = BotPose[1]; + + if(alliance.get() == Alliance.Blue){ + y = 7.380; + x = 2.305; + angle = 0; + if(limelight3g.getV()){ + if(pigeon2.getYaw().getValueAsDouble() >355 || pigeon2.getYaw().getValueAsDouble() < 5){ + if((y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05)){ + drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0)); + } + else{ + if(botx < 6){ + drivetrain.setControl(drive.withVelocityX(y-boty)); + } + else{ + drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); + } + } + } + else{ + if(pigeon2.getYaw().getValueAsDouble() >angle && pigeon2.getYaw().getValueAsDouble() =angle +180){ + + drivetrain.setControl(drive.withRotationalRate(-0.5)); + } + } + } + } + else{ + y = 0.639; + x = 13.963; + angle = 180; + if(limelight3g.getV()){ + if(pigeon2.getYaw().getValueAsDouble() >175 && pigeon2.getYaw().getValueAsDouble() < 185){ + if((y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05)){ + drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0)); + } + else{ + if(botx > 10){ + drivetrain.setControl(drive.withVelocityX(y-boty)); + } + else{ + drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); + } + } + } + else{ + 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) {} + public void end(boolean interrupted) { + drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0)); + } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return (y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-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 index a2831d9..a24e018 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java +++ b/src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java @@ -66,6 +66,6 @@ public class TournerVersMur extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return pigeon2.getYaw().getValueAsDouble() > angle - 5 && pigeon2.getYaw().getValueAsDouble() < angle + 5; + return pigeon2.getYaw().getValueAsDouble() > angle && pigeon2.getYaw().getValueAsDouble() < angle + 5; } } diff --git a/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java index eefc789..125dda7 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java +++ b/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java @@ -66,6 +66,6 @@ public class TournerVersReservoir extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return pigeon2.getYaw().getValueAsDouble() > angle - 5 && pigeon2.getYaw().getValueAsDouble() < angle + 5; + return pigeon2.getYaw().getValueAsDouble() > angle && pigeon2.getYaw().getValueAsDouble() < angle + 5; } }