From 10b01103157b0cd64bcacb80a079d4854d61f0db Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Tue, 31 Mar 2026 17:53:41 -0400 Subject: [PATCH] mode auto --- .../autos/Copy of DepotTirerReservoir.auto | 99 +++++++++++++++++++ .../pathplanner/autos/DepotTirerMur.auto | 6 ++ .../autos/DepotTirerReservoir.auto | 6 ++ .../pathplanner/paths/MonterReservoir.path | 16 +-- src/main/java/frc/robot/RobotContainer.java | 1 + .../java/frc/robot/commands/Limelighter.java | 22 ++--- .../robot/commands/ModeAuto/GrimperMur.java | 2 +- .../commands/ModeAuto/GrimperReservoir.java | 84 +++++++++------- .../commands/ModeAuto/LimelighterAuto.java | 2 - .../ModeAuto/TournerVersReservoir.java | 11 ++- .../frc/robot/commands/MonterGrimpeur.java | 2 +- .../frc/robot/subsystems/Limelight3G.java | 8 +- 12 files changed, 190 insertions(+), 69 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Copy of DepotTirerReservoir.auto diff --git a/src/main/deploy/pathplanner/autos/Copy of DepotTirerReservoir.auto b/src/main/deploy/pathplanner/autos/Copy of DepotTirerReservoir.auto new file mode 100644 index 0000000..de04b62 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Copy of DepotTirerReservoir.auto @@ -0,0 +1,99 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "DescendreBalayeuse" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "path", + "data": { + "pathName": "Depot" + } + }, + { + "type": "named", + "data": { + "name": "Aspirer" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Tir" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Lancer" + } + }, + { + "type": "named", + "data": { + "name": "Aspirer" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "GrimperReservoir" + } + }, + { + "type": "named", + "data": { + "name": "GrimperReservoir" + } + }, + { + "type": "named", + "data": { + "name": "MonterGrimpeur" + } + }, + { + "type": "path", + "data": { + "pathName": "MonterReservoir" + } + }, + { + "type": "named", + "data": { + "name": "DescendreGrimpeur" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/DepotTirerMur.auto b/src/main/deploy/pathplanner/autos/DepotTirerMur.auto index 326fc3d..dc157ad 100644 --- a/src/main/deploy/pathplanner/autos/DepotTirerMur.auto +++ b/src/main/deploy/pathplanner/autos/DepotTirerMur.auto @@ -14,6 +14,12 @@ "type": "deadline", "data": { "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto b/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto index 0f80659..15e36ff 100644 --- a/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto +++ b/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto @@ -14,6 +14,12 @@ "type": "deadline", "data": { "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/paths/MonterReservoir.path b/src/main/deploy/pathplanner/paths/MonterReservoir.path index 64b0950..85ada95 100644 --- a/src/main/deploy/pathplanner/paths/MonterReservoir.path +++ b/src/main/deploy/pathplanner/paths/MonterReservoir.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.0502282453637657, - "y": 2.4758915834522113 + "x": 1.0, + "y": 2.6 }, "prevControl": null, "nextControl": { - "x": 1.0408884389937225, - "y": 2.742159765250622 + "x": 0.9906601936299568, + "y": 2.866268181798411 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.0502282453637657, - "y": 2.7734807417974334 + "x": 1.0, + "y": 2.9 }, "prevControl": { - "x": 1.0547546338516156, - "y": 2.523521721541598 + "x": 1.00452638848785, + "y": 2.6500409797441646 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 59e696e..db5966a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -126,6 +126,7 @@ public class RobotContainer { manette1.b().whileTrue(new ModeOposer(lanceur)); manette1.a().whileTrue(new ModeOposerDemeleur(lanceur)); manette1.y().whileTrue(new ModeOposerBalayeuse(balayeuse)); + manette1.povUp().whileTrue(new LancerAuto(lanceur, limeLight3G)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/Limelighter.java b/src/main/java/frc/robot/commands/Limelighter.java index a7be2f3..6aa6453 100644 --- a/src/main/java/frc/robot/commands/Limelighter.java +++ b/src/main/java/frc/robot/commands/Limelighter.java @@ -56,7 +56,7 @@ public class Limelighter extends Command { return; } if (alliance.get() == Alliance.Blue) { - x = 4.6; + x = 4; } else { x = 11.915394; @@ -64,30 +64,28 @@ public class Limelighter extends Command { botx = BotPose[1]; boty = BotPose[0]; angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); - calcul = limelight3g.Calcule(botx, x, boty, 4, angle); - if(calcul < -5 && calcul > -180){ + calcul = limelight3g.Calcule(botx, x, boty, 4.6, angle); + if(calcul < -3 && calcul > -180){ drivetrain.setControl( - drive.withRotationalRate(0.5*(2*Math.PI))); + drive.withRotationalRate(-1.5)); } - else if(calcul > 5 && calcul < 180){ + else if(calcul > 3 && calcul < 180){ drivetrain.setControl( - drive.withRotationalRate(-0.5*(2*Math.PI))); + drive.withRotationalRate(-1.5)); } else if(calcul >= 180){ drivetrain.setControl( - drive.withRotationalRate(-0.5*(2*Math.PI))); + drive.withRotationalRate(-1.5)); } else if(calcul <= -180){ drivetrain.setControl( - drive.withRotationalRate(0.5*(2*Math.PI))); + drive.withRotationalRate(1.5)); } else{ drivetrain.setControl( drive.withRotationalRate(0)); - } - drivetrain.setControl( - drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); - System.out.println(angle); + } + System.out.println(calcul); if (calcul < 0.2 && calcul > -0.2) { drivetrain.setControl(drive.withRotationalRate(0)); } diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java index 768028c..a801c51 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java @@ -60,7 +60,7 @@ public class GrimperMur extends Command { angle = angle + 360; } if(alliance.get() == Alliance.Blue){ - y = 2.961328; + y = 5; x = 1.11; angle = 0; if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){ diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java index 381b213..c9a5ad5 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java @@ -14,6 +14,7 @@ import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; @@ -52,46 +53,53 @@ public class GrimperReservoir extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - BotPose = limeLight3G.getBotPoseBlue(); - botx = BotPose[0]; - boty = BotPose[1]; - System.out.println(drivetrain.getPigeon2().getYaw().getValueAsDouble()); - if(angle < 0){ - angle = angle + 360; - } - if(alliance.get() == Alliance.Red){ - y = 6.959326; - x = 13.57966; - angle = 180; - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190 && drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170){ - drivetrain.setControl(drive.withVelocityY(y-boty).withVelocityX(x-botx)); + if(limeLight3G.getV()){ + + BotPose = limeLight3G.getBotPoseBlue(); + botx = BotPose[0]; + boty = BotPose[1]; + System.out.println(drivetrain.getPigeon2().getYaw().getValueAsDouble()); + if(angle < 0){ + angle = angle + 360; + } + if(alliance.get() == Alliance.Red){ + y = 6.959326; + x = 13.57966; + angle = 180; + if(drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190 && drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170){ + drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5),-2,2))); + } + else{ + if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ + drivetrain.setControl(drive.withRotationalRate(1)); + } + else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ + drivetrain.setControl(drive.withRotationalRate(-1)); + } + } + } + else{ + y = 2.6; + x = 1.11; + angle = 0; + if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 358 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 2){ + drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5),-2,2)).withRotationalRate(0)); + } + else{ + if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ + drivetrain.setControl(drive.withRotationalRate(-1)); + System.out.println("x"); + } + else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ + System.out.println("e"); + drivetrain.setControl(drive.withRotationalRate(1)); + } + } + } } else{ - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI)); - } - else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI)); - } + drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0)); } - } - else{ - y = 5.081328; - x = 1.11; - angle = 0; - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){ - drivetrain.setControl(drive.withVelocityY(y-boty).withVelocityX(x-botx)); - } - else{ - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI)); - } - else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI)); - } - } - } - } // Called once the command ends or is interrupted. @@ -103,6 +111,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 (y-boty < 0.1 && y-boty >-0.1) && (x-botx < 0.1 && x-botx > -0.1); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java index 31ab317..d551177 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java @@ -85,8 +85,6 @@ public class LimelighterAuto extends Command { drivetrain.setControl( drive.withRotationalRate(0)); } - drivetrain.setControl( - drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); System.out.println(angle); if (calcul < 0.2 && calcul > -0.2) { drivetrain.setControl(drive.withRotationalRate(0)); diff --git a/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java index 0e537eb..32946e2 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java +++ b/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java @@ -52,13 +52,18 @@ public class TournerVersReservoir extends Command { angle = 0; } } + if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > angle && drivetrain.getPigeon2().getYaw().getValueAsDouble() < angle + 10){ + drivetrain.setControl(drive.withRotationalRate(0)); + } + else{ if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(-force*180/Math.PI)); + drivetrain.setControl(drive.withRotationalRate(-force*2)); } else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(force*180/Math.PI)); + drivetrain.setControl(drive.withRotationalRate(force*2)); } } + } // Called once the command ends or is interrupted. @Override @@ -67,6 +72,6 @@ public class TournerVersReservoir extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return drivetrain.getPigeon2().getYaw().getValueAsDouble() > angle && drivetrain.getPigeon2().getYaw().getValueAsDouble() < angle + 10; + return drivetrain.getPigeon2().getYaw().getValueAsDouble() > angle && drivetrain.getPigeon2().getYaw().getValueAsDouble() < angle + 10; } } diff --git a/src/main/java/frc/robot/commands/MonterGrimpeur.java b/src/main/java/frc/robot/commands/MonterGrimpeur.java index a8a3e84..d79b763 100644 --- a/src/main/java/frc/robot/commands/MonterGrimpeur.java +++ b/src/main/java/frc/robot/commands/MonterGrimpeur.java @@ -42,6 +42,6 @@ public class MonterGrimpeur extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return Math.abs(grimpeur.Position()) > grimpeur.PositionFinal(); } } diff --git a/src/main/java/frc/robot/subsystems/Limelight3G.java b/src/main/java/frc/robot/subsystems/Limelight3G.java index da4ce72..265cba1 100644 --- a/src/main/java/frc/robot/subsystems/Limelight3G.java +++ b/src/main/java/frc/robot/subsystems/Limelight3G.java @@ -54,18 +54,18 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag") { if(x1 > x2){ if(y1 > y2){ - return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI) - angle; + return Math.atan((x2 - x1) / (y2 - y1)* (180/Math.PI)) - angle; } else{ - return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)+90 - angle; + return Math.atan((x2 - x1) / (y2 - y1)* (180/Math.PI)) + 180 - angle; } } else{ if(y1 > y2){ - return Math.atan((x2 - x1) / (y2 - y1))* (180 / Math.PI)+270 - angle; + return 90 -(Math.atan((x2 - x1) / (y2 - y1)) * (180/Math.PI)) + 270 - angle; } else{ - return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI)+180 - angle; + return Math.atan((x2 - x1) / (y2 - y1)* (180/Math.PI)) - angle; } } }