From b0ad34519e11ffab96482937015e4e30dfc299c9 Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Wed, 1 Apr 2026 19:27:12 -0400 Subject: [PATCH] mode auto --- .../pathplanner/autos/DepotTirerMur.auto | 23 +++++++++++------ .../autos/DepotTirerReservoir.auto | 23 +++++++++++------ .../java/frc/robot/commands/Inverser.java | 8 +++--- .../robot/commands/ModeAuto/GrimperMur.java | 4 ++- .../commands/ModeAuto/LimelighterAuto.java | 25 ++++++++++--------- 5 files changed, 49 insertions(+), 34 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/DepotTirerMur.auto b/src/main/deploy/pathplanner/autos/DepotTirerMur.auto index e157521..d5346e4 100644 --- a/src/main/deploy/pathplanner/autos/DepotTirerMur.auto +++ b/src/main/deploy/pathplanner/autos/DepotTirerMur.auto @@ -48,15 +48,22 @@ } }, { - "type": "named", + "type": "deadline", "data": { - "name": "Lancer" - } - }, - { - "type": "named", - "data": { - "name": "TournerA180" + "commands": [ + { + "type": "named", + "data": { + "name": "Lancer" + } + }, + { + "type": "named", + "data": { + "name": "Aspirer" + } + } + ] } }, { diff --git a/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto b/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto index 62f3ffc..b73aedd 100644 --- a/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto +++ b/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto @@ -48,15 +48,22 @@ } }, { - "type": "named", + "type": "deadline", "data": { - "name": "Lancer" - } - }, - { - "type": "named", - "data": { - "name": "TournerAZero" + "commands": [ + { + "type": "named", + "data": { + "name": "Lancer" + } + }, + { + "type": "named", + "data": { + "name": "Aspirer" + } + } + ] } }, { diff --git a/src/main/java/frc/robot/commands/Inverser.java b/src/main/java/frc/robot/commands/Inverser.java index 3b8f0a0..76ccf45 100644 --- a/src/main/java/frc/robot/commands/Inverser.java +++ b/src/main/java/frc/robot/commands/Inverser.java @@ -19,15 +19,13 @@ public class Inverser extends Command { // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + drivetrain.getPigeon2().setYaw(drivetrain.getPigeon2().getYaw().getValueAsDouble()+180); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(!inverse){ - drivetrain.getPigeon2().setYaw(drivetrain.getPigeon2().getYaw().getValueAsDouble()+180); - inverse = true; - } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java index 5eb1bf1..810c461 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; 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 */ public class GrimperMur extends Command { @@ -57,6 +58,7 @@ public class GrimperMur extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + if(limeLight3.getV()){ BotPose = limeLight3.getBotPoseBlue(); botx = BotPose[0]; boty = BotPose[1]; @@ -96,7 +98,7 @@ public class GrimperMur extends Command { } } } - + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java index 8175fc7..9cddbe6 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java @@ -53,7 +53,7 @@ public class LimelighterAuto extends Command { double[] BotPose = new double[6]; System.out.println("e"); if (limelight3g.getV()) { - // BotPose = limelight3g.getBotPoseBlue(); + BotPose = limelight3g.getBotPoseBlue(); if (!alliance.isPresent()) { return; } @@ -65,25 +65,25 @@ public class LimelighterAuto extends Command { x = 11.915394; BotPose = limelight3g.getBotPoseRed(); } - botx = BotPose[1]; - boty = BotPose[0]; - angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); + botx = BotPose[0]; + boty = BotPose[1]; + angle = BotPose[5]; calcul = limelight3g.Calcule(botx, x, boty, 4, angle); - if(angle > 180){ - angle -= 360; - } if(calcul > -5 && calcul < 5){ drivetrain.setControl( drive.withRotationalRate(0)); } else if(calcul > 5){ drivetrain.setControl( - drive.withRotationalRate(0.5*(2*Math.PI))); + drive.withRotationalRate(2)); } else if(calcul < -5){ - drivetrain.setControl( - drive.withRotationalRate(-0.5*(2*Math.PI))); + drivetrain.setControl(drive.withRotationalRate(-2)); } + // botx = BotPose[1]; + // boty = BotPose[0]; + // angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); + // calcul = limelight3g.Calcule(botx, x, boty, 4, angle); // if(calcul < -5 && calcul > -180){ // drivetrain.setControl( // drive.withRotationalRate(0.5*(2*Math.PI))); @@ -92,7 +92,7 @@ public class LimelighterAuto extends Command { // drivetrain.setControl( // drive.withRotationalRate(-0.5*(2*Math.PI))); // } - // else if(calcul >= 180){ + // else if(calcul < -5){ // drivetrain.setControl( // drive.withRotationalRate(-0.5*(2*Math.PI))); // } @@ -126,6 +126,7 @@ public class LimelighterAuto extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return calcul > -5 && calcul < 5; + return calcul > -5 && calcul < 5; } } +