From 10b01103157b0cd64bcacb80a079d4854d61f0db Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Tue, 31 Mar 2026 17:53:41 -0400 Subject: [PATCH 1/7] 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; } } } From 9d09af20b032dca8cc7676c6e92b9a1cccd20c86 Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Tue, 31 Mar 2026 19:11:09 -0400 Subject: [PATCH 2/7] vfdrty --- .../autos/Copy of DepotTirerReservoir.auto | 4 +- .../pathplanner/autos/DepotTirerMur.auto | 4 +- .../autos/DepotTirerReservoir.auto | 4 +- src/main/java/frc/robot/RobotContainer.java | 7 +- .../java/frc/robot/commands/Limelighter.java | 61 ++++++++------ .../commands/ModeAuto/BougerDroiteAuto.java | 80 +++++++++++++++++++ .../commands/ModeAuto/BougerGaucheAuto.java | 79 ++++++++++++++++++ .../robot/commands/ModeAuto/GrimperMur.java | 5 +- .../commands/ModeAuto/GrimperReservoir.java | 7 +- .../commands/ModeAuto/LimelighterAuto.java | 2 +- .../subsystems/CommandSwerveDrivetrain.java | 13 ++- src/main/java/frc/robot/subsystems/Led.java | 13 +-- .../frc/robot/subsystems/Limelight3G.java | 2 +- 13 files changed, 232 insertions(+), 49 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ModeAuto/BougerDroiteAuto.java create mode 100644 src/main/java/frc/robot/commands/ModeAuto/BougerGaucheAuto.java diff --git a/src/main/deploy/pathplanner/autos/Copy of DepotTirerReservoir.auto b/src/main/deploy/pathplanner/autos/Copy of DepotTirerReservoir.auto index de04b62..2b0de62 100644 --- a/src/main/deploy/pathplanner/autos/Copy of DepotTirerReservoir.auto +++ b/src/main/deploy/pathplanner/autos/Copy of DepotTirerReservoir.auto @@ -79,9 +79,9 @@ } }, { - "type": "path", + "type": "named", "data": { - "pathName": "MonterReservoir" + "name": "gauche" } }, { diff --git a/src/main/deploy/pathplanner/autos/DepotTirerMur.auto b/src/main/deploy/pathplanner/autos/DepotTirerMur.auto index dc157ad..e157521 100644 --- a/src/main/deploy/pathplanner/autos/DepotTirerMur.auto +++ b/src/main/deploy/pathplanner/autos/DepotTirerMur.auto @@ -78,9 +78,9 @@ } }, { - "type": "path", + "type": "named", "data": { - "pathName": "MonterMur" + "name": "droite" } }, { diff --git a/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto b/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto index 15e36ff..62f3ffc 100644 --- a/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto +++ b/src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto @@ -78,9 +78,9 @@ } }, { - "type": "path", + "type": "named", "data": { - "pathName": "MonterReservoir" + "name": "gauche" } }, { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index db5966a..ce353e8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,6 +30,8 @@ import frc.robot.commands.ModeOposerDemeleur; import frc.robot.commands.MonterBalyeuse; import frc.robot.commands.MonterGrimpeur; import frc.robot.commands.ModeAuto.AspirerAuto; +import frc.robot.commands.ModeAuto.BougerDroiteAuto; +import frc.robot.commands.ModeAuto.BougerGaucheAuto; import frc.robot.commands.ModeAuto.GrimperMur; import frc.robot.commands.ModeAuto.GrimperReservoir; import frc.robot.commands.ModeAuto.LancerAuto; @@ -73,6 +75,8 @@ public class RobotContainer { public RobotContainer() { + NamedCommands.registerCommand("droite", new BougerDroiteAuto(drivetrain)); + NamedCommands.registerCommand("gauche", new BougerGaucheAuto(drivetrain)); NamedCommands.registerCommand("GrimperMur", new GrimperMur(limeLight3,drivetrain)); NamedCommands.registerCommand("GrimperReservoir", new GrimperReservoir(limeLight3G,drivetrain)); NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur, limeLight3G)); @@ -126,7 +130,8 @@ 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)); + manette1.povRight().whileTrue(new BougerDroiteAuto(drivetrain)); + manette1.povLeft().whileTrue(new BougerGaucheAuto(drivetrain)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/Limelighter.java b/src/main/java/frc/robot/commands/Limelighter.java index 1d0acbc..811f3ec 100644 --- a/src/main/java/frc/robot/commands/Limelighter.java +++ b/src/main/java/frc/robot/commands/Limelighter.java @@ -53,7 +53,7 @@ public class Limelighter 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,36 +65,47 @@ public class Limelighter extends Command { x = 11.915394; BotPose = limelight3g.getBotPoseRed(); } - 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))); + if(calcul > -5 && calcul < 5){ + drivetrain.setControl( + drive.withRotationalRate(0)); } - else if(calcul > 5 && calcul < 180){ + else if(calcul > 5){ drivetrain.setControl( drive.withRotationalRate(-0.5*(2*Math.PI))); } else if(calcul < -5){ - drivetrain.setControl( - drive.withRotationalRate(-0.5*(2*Math.PI))); - } - else if(calcul <= -180){ - drivetrain.setControl( - drive.withRotationalRate(0.5*(2*Math.PI))); - } - else{ - 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)); + drivetrain.setControl(drive.withRotationalRate(-0.5*(2*Math.PI))); } + // 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))); + // } + // else if(calcul > 5 && calcul < 180){ + // drivetrain.setControl( + // drive.withRotationalRate(-0.5*(2*Math.PI))); + // } + // else if(calcul < -5){ + // drivetrain.setControl( + // drive.withRotationalRate(-0.5*(2*Math.PI))); + // } + // else if(calcul <= -180){ + // drivetrain.setControl( + // drive.withRotationalRate(0.5*(2*Math.PI))); + // } + // else{ + // 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)); + // } } else{ drivetrain.setControl(drive.withRotationalRate(0)); diff --git a/src/main/java/frc/robot/commands/ModeAuto/BougerDroiteAuto.java b/src/main/java/frc/robot/commands/ModeAuto/BougerDroiteAuto.java new file mode 100644 index 0000000..72952e7 --- /dev/null +++ b/src/main/java/frc/robot/commands/ModeAuto/BougerDroiteAuto.java @@ -0,0 +1,80 @@ +// 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 static edu.wpi.first.units.Units.*; + +import java.util.Optional; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; + +/* 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 BougerDroiteAuto extends Command { + CommandSwerveDrivetrain drivetrain; + Timer timer = new Timer(); + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); + Optional alliance; + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + /** Creates a new AvanceAuto. */ + public BougerDroiteAuto(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() { + alliance = DriverStation.getAlliance(); + timer.reset(); + timer.start(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(alliance.get() == Alliance.Blue){ + if(timer.get() < .75){ + System.out.println("8765"); + drivetrain.setControl(drive.withVelocityY(-1.5)); + } + else{ + drivetrain.setControl(drive.withVelocityY(0)); + } + } + else{ + if(timer.get() < 0.75){ + drivetrain.setControl(drive.withVelocityY(1.5)); + } + else{ + drivetrain.setControl(drive.withVelocityY(0)); + } + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + drivetrain.setControl(drive.withVelocityY(0)); + timer.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return timer.get() > 1; + } +} diff --git a/src/main/java/frc/robot/commands/ModeAuto/BougerGaucheAuto.java b/src/main/java/frc/robot/commands/ModeAuto/BougerGaucheAuto.java new file mode 100644 index 0000000..e4a3748 --- /dev/null +++ b/src/main/java/frc/robot/commands/ModeAuto/BougerGaucheAuto.java @@ -0,0 +1,79 @@ +// 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 static edu.wpi.first.units.Units.*; + +import java.util.Optional; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; + +/* 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 BougerGaucheAuto extends Command { + CommandSwerveDrivetrain drivetrain; + Timer timer = new Timer(); + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); + Optional alliance; + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + /** Creates a new AvanceAuto. */ + public BougerGaucheAuto(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() { + alliance = DriverStation.getAlliance(); + timer.reset(); + timer.start(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(alliance.get() == Alliance.Blue){ + if(timer.get() < 0.75){ + drivetrain.setControl(drive.withVelocityY(1.5)); + } + else{ + drivetrain.setControl(drive.withVelocityY(0)); + } + } + else{ + if(timer.get() < 0.75){ + drivetrain.setControl(drive.withVelocityY(-1.5)); + } + else{ + drivetrain.setControl(drive.withVelocityY(0)); + } + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + drivetrain.setControl(drive.withVelocityY(0)); + timer.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + 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 b7e1255..5eb1bf1 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java @@ -48,7 +48,10 @@ public class GrimperMur extends Command { // Called when the command is initially scheduled. @Override public void initialize() { - alliance = DriverStation.getAlliance(); + // alliance = DriverStation.getAlliance(); + // if(drivetrain.Equipe()){ + // angle+=180; + // } } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java index 5dbdba1..62e4cb7 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java @@ -49,7 +49,10 @@ public class GrimperReservoir extends Command { // Called when the command is initially scheduled. @Override public void initialize() { - alliance = DriverStation.getAlliance(); + // alliance = DriverStation.getAlliance(); + // if(drivetrain.Equipe()){ + // drivetrain.getPigeon2().setYaw(drivetrain.getPigeon2().getYaw().getValueAsDouble()+180); + // } } // Called every time the scheduler runs while the command is scheduled. @@ -85,7 +88,7 @@ public class GrimperReservoir extends Command { 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)); + drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)).withRotationalRate(0)); } else{ if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ diff --git a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java index 21cb1e8..8175fc7 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java @@ -78,7 +78,7 @@ public class LimelighterAuto extends Command { } else if(calcul > 5){ drivetrain.setControl( - drive.withRotationalRate(-0.5*(2*Math.PI))); + drive.withRotationalRate(0.5*(2*Math.PI))); } else if(calcul < -5){ drivetrain.setControl( diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 9b70aea..c20b5f1 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -22,10 +22,14 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -54,8 +58,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su /* Swerve requests to apply during SysId characterization */ private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); + ShuffleboardTab teb = Shuffleboard.getTab("teb"); private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - + private GenericEntry equipe = + teb.add("equipe commence (bleu = ouvert, rouge = fermé)", true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry(); /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( new SysIdRoutine.Config( @@ -221,7 +227,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su } configureAutoBuilder(); } - + public boolean Equipe(){ + return equipe.getBoolean(false); + } /** * Returns a command that applies the specified control request to this swerve drivetrain. * @@ -254,7 +262,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su else if(getPigeon2().getYaw().getValueAsDouble() < 0){ getPigeon2().setYaw(getPigeon2().getYaw().getValueAsDouble()+360); } - /* * Periodically try to apply the operator perspective. * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. diff --git a/src/main/java/frc/robot/subsystems/Led.java b/src/main/java/frc/robot/subsystems/Led.java index f628715..8cacbf2 100644 --- a/src/main/java/frc/robot/subsystems/Led.java +++ b/src/main/java/frc/robot/subsystems/Led.java @@ -16,8 +16,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Led extends SubsystemBase { ShuffleboardTab teb = Shuffleboard.getTab("teb"); - private GenericEntry equipe = - teb.add("equipe commence (bleu = ouvert, rouge = fermé)", true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry(); @SuppressWarnings("removal") CANdle CANDle = new CANdle(17); @SuppressWarnings("removal") @@ -91,9 +89,6 @@ public class Led extends SubsystemBase { public void RainBowStop(){ CANDle.animate(null); } - public boolean Equipe(){ - return equipe.getBoolean(true); - } /** Creates a new Led. */ public Led() {} @@ -104,7 +99,7 @@ public class Led extends SubsystemBase { if(temps > 20 && temps < 30){ Vert1(); } - if(Equipe()){ + // if(Equipe()){ if(temps > 30 && temps < 55){ bleu(); } @@ -120,8 +115,8 @@ public class Led extends SubsystemBase { else{ RainBow(); } - } - else{ + // } + // else{ if(temps > 30 && temps < 55){ Rouge(); } @@ -140,6 +135,6 @@ public class Led extends SubsystemBase { } // This method will be called once per scheduler run - } + // } } diff --git a/src/main/java/frc/robot/subsystems/Limelight3G.java b/src/main/java/frc/robot/subsystems/Limelight3G.java index 5bc3bb6..1303b8f 100644 --- a/src/main/java/frc/robot/subsystems/Limelight3G.java +++ b/src/main/java/frc/robot/subsystems/Limelight3G.java @@ -72,7 +72,7 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag") return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle; } else{ - return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)-360 - angle; + return -Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle; } } @Override From 4d1b353e25bcec5cc4effd177c647e1eed221bfd Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Tue, 31 Mar 2026 19:20:34 -0400 Subject: [PATCH 3/7] limelighter coriger --- src/main/java/frc/robot/commands/Limelighter.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/commands/Limelighter.java b/src/main/java/frc/robot/commands/Limelighter.java index 811f3ec..345c982 100644 --- a/src/main/java/frc/robot/commands/Limelighter.java +++ b/src/main/java/frc/robot/commands/Limelighter.java @@ -65,6 +65,9 @@ public class Limelighter extends Command { x = 11.915394; BotPose = limelight3g.getBotPoseRed(); } + botx = BotPose[0]; + boty = BotPose[1]; + calcul = limelight3g.Calcule(botx, x, boty, 4, angle); if(calcul > -5 && calcul < 5){ drivetrain.setControl( drive.withRotationalRate(0)); From 52c72e9a6153a04e977a4760b0aafbb7d96bad19 Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Tue, 31 Mar 2026 19:52:24 -0400 Subject: [PATCH 4/7] inverser --- src/main/java/frc/robot/RobotContainer.java | 2 + .../java/frc/robot/commands/Inverser.java | 42 +++++++++++++++++++ 2 files changed, 44 insertions(+) create mode 100644 src/main/java/frc/robot/commands/Inverser.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ce353e8..f4d6ba0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.Aspirer; import frc.robot.commands.DescendreBalyeuse; import frc.robot.commands.DescendreGrimpeur; +import frc.robot.commands.Inverser; import frc.robot.commands.Lancer; import frc.robot.commands.LancerAspirer; import frc.robot.commands.LancerBaseVitesse; @@ -132,6 +133,7 @@ public class RobotContainer { manette1.y().whileTrue(new ModeOposerBalayeuse(balayeuse)); manette1.povRight().whileTrue(new BougerDroiteAuto(drivetrain)); manette1.povLeft().whileTrue(new BougerGaucheAuto(drivetrain)); + manette1.start().whileTrue(new Inverser(drivetrain)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/Inverser.java b/src/main/java/frc/robot/commands/Inverser.java new file mode 100644 index 0000000..3b8f0a0 --- /dev/null +++ b/src/main/java/frc/robot/commands/Inverser.java @@ -0,0 +1,42 @@ +// 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; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.CommandSwerveDrivetrain; + +/* 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 Inverser extends Command { + CommandSwerveDrivetrain drivetrain; + boolean inverse = false; + /** Creates a new Inverser. */ + public Inverser(CommandSwerveDrivetrain 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(!inverse){ + drivetrain.getPigeon2().setYaw(drivetrain.getPigeon2().getYaw().getValueAsDouble()+180); + inverse = true; + } + } + + // 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 false; + } +} From 272f89962a9d87a9706681d663773c1355a421ac Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Tue, 31 Mar 2026 19:53:25 -0400 Subject: [PATCH 5/7] mode marche yesssssssss --- src/main/java/frc/robot/commands/Lancer.java | 7 +++++-- .../commands/ModeAuto/GrimperReservoir.java | 17 +++++++++-------- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/commands/Lancer.java b/src/main/java/frc/robot/commands/Lancer.java index e29f8b1..ace73e9 100644 --- a/src/main/java/frc/robot/commands/Lancer.java +++ b/src/main/java/frc/robot/commands/Lancer.java @@ -48,7 +48,9 @@ public class Lancer extends Command { public void execute() { double[] BotPose = new double[6]; if(limeLight3G.getV()){ - if(!alliance.isPresent()){return;} + if(!alliance.isPresent()){ + return; + } if(alliance.get() == Alliance.Blue){ BotPose = limeLight3G.getBotPoseBlue(); } @@ -60,10 +62,11 @@ public class Lancer extends Command { vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250; } if(limeLight3G.getV()){ - System.out.println(vitesse); + double output = pidController.calculate(lanceur.Vitesse(),vitesse); lanceur.Lancer(output); + System.out.println(output); if(lanceur.Vitesse() >= vitesse-800){ timer.start(); if(timer.get() >1){ diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java index 62e4cb7..0b0adf6 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java @@ -32,6 +32,7 @@ public class GrimperReservoir extends Command { double x; double y; double angle; + double pigeonAngle; private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); Optional alliance = DriverStation.getAlliance(); @@ -59,11 +60,11 @@ public class GrimperReservoir extends Command { @Override public void execute() { if(limeLight3G.getV()){ - BotPose = limeLight3G.getBotPoseBlue(); botx = BotPose[0]; boty = BotPose[1]; - System.out.println(drivetrain.getPigeon2().getYaw().getValueAsDouble()); + pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble()+180; + System.out.println(pigeonAngle); if(angle < 0){ angle = angle + 360; } @@ -71,14 +72,14 @@ public class GrimperReservoir extends Command { y = 6.959326; x = 13.57966; angle = 180; - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190 && drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170){ + if(pigeonAngle< 190 && pigeonAngle> 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){ + if(pigeonAngle>0 && pigeonAngle<180){ drivetrain.setControl(drive.withRotationalRate(1)); } - else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ + else if(pigeonAngle>180){ drivetrain.setControl(drive.withRotationalRate(-1)); } } @@ -87,15 +88,15 @@ public class GrimperReservoir extends Command { y = 2.6; x = 1.11; angle = 0; - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 358 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 2){ + if(pigeonAngle> 358 || pigeonAngle< 2){ drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)).withRotationalRate(0)); } else{ - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ + if(pigeonAngle>0 && pigeonAngle<180){ drivetrain.setControl(drive.withRotationalRate(-1)); System.out.println("x"); } - else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ + else if(pigeonAngle>180){ System.out.println("e"); drivetrain.setControl(drive.withRotationalRate(1)); } From 7b535a845fc7c04da988a6413b87f337008afe9e Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Tue, 31 Mar 2026 19:59:15 -0400 Subject: [PATCH 6/7] grimpe en rouge aussi --- .../commands/ModeAuto/GrimperReservoir.java | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java index 0b0adf6..03e8629 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java @@ -63,8 +63,6 @@ public class GrimperReservoir extends Command { BotPose = limeLight3G.getBotPoseBlue(); botx = BotPose[0]; boty = BotPose[1]; - pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble()+180; - System.out.println(pigeonAngle); if(angle < 0){ angle = angle + 360; } @@ -85,23 +83,25 @@ public class GrimperReservoir extends Command { } } else{ + pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble()+180; + System.out.println(pigeonAngle); y = 2.6; - x = 1.11; - angle = 0; - if(pigeonAngle> 358 || pigeonAngle< 2){ - drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)).withRotationalRate(0)); - } - else{ - if(pigeonAngle>0 && pigeonAngle<180){ - drivetrain.setControl(drive.withRotationalRate(-1)); - System.out.println("x"); + x = 1.11; + angle = 0; + if(pigeonAngle> 358 || pigeonAngle< 2){ + drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)).withRotationalRate(0)); } - else if(pigeonAngle>180){ - System.out.println("e"); - drivetrain.setControl(drive.withRotationalRate(1)); + else{ + if(pigeonAngle>0 && pigeonAngle<180){ + drivetrain.setControl(drive.withRotationalRate(-1)); + System.out.println("x"); + } + else if(pigeonAngle>180){ + System.out.println("e"); + drivetrain.setControl(drive.withRotationalRate(1)); + } } } - } } else{ drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0)); From 3b06cbd447eabd26829b5e1ba1edf4b8743fe5d7 Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Tue, 31 Mar 2026 20:29:45 -0400 Subject: [PATCH 7/7] led --- .../frc/robot/subsystems/LEDSubsystem.java | 39 +++++++++++++++++++ src/main/java/frc/robot/subsystems/Led.java | 5 --- 2 files changed, 39 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/LEDSubsystem.java diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java new file mode 100644 index 0000000..9bb2c7d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -0,0 +1,39 @@ +/* Generated by Phoenix Tuner X */ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.controls.SolidColor; +import com.ctre.phoenix6.hardware.CANdle; +import com.ctre.phoenix6.signals.RGBWColor; + +/** + * Subsystem that controls an addressable LED strip using a CANdle. + */ +public class LEDSubsystem extends SubsystemBase { + private final CANBus kCANBus = new CANBus("rio"); + private final CANdle m_candle = new CANdle(17, kCANBus); + public void Bleu(){ + m_candle.setControl(new SolidColor(0, 80).withColor(new RGBWColor(255, 0, 0, 0))); + } + public void Rouge(){ + new SolidColor(0, 80).withColor(new RGBWColor(0, 0, 255, 0)); + } + public LEDSubsystem() { + setDefaultCommand(updateLEDs()); + } + + /** + * Updates the animations and LEDs of the CANdle. + * + * @return Command to run + */ + public Command updateLEDs() { + return run(() -> {}); + } +} + diff --git a/src/main/java/frc/robot/subsystems/Led.java b/src/main/java/frc/robot/subsystems/Led.java index 8cacbf2..da22522 100644 --- a/src/main/java/frc/robot/subsystems/Led.java +++ b/src/main/java/frc/robot/subsystems/Led.java @@ -16,11 +16,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Led extends SubsystemBase { ShuffleboardTab teb = Shuffleboard.getTab("teb"); - @SuppressWarnings("removal") CANdle CANDle = new CANdle(17); - @SuppressWarnings("removal") RainbowAnimation rainbowAnim = new RainbowAnimation(); - @SuppressWarnings("removal") public void bleu(){ CANDle.setLEDs(0, 0, 255,0,0,8); CANDle.setLEDs(0, 0, 255,0,16,8); @@ -45,7 +42,6 @@ public class Led extends SubsystemBase { // CANDle.setLEDs(0, 255, 0,0,120,8); // CANDle.setLEDs(0, 255, 0,0,136,8); } - @SuppressWarnings("removal") public void Rouge(){ CANDle.setLEDs(255, 0, 0,0,0,8); CANDle.setLEDs(255, 0, 0,0,16,8); @@ -57,7 +53,6 @@ public class Led extends SubsystemBase { // CANDle.setLEDs(255, 0, 0,0,112,8); // CANDle.setLEDs(255, 0, 0,0,128,8); } - @SuppressWarnings("removal") public void Jaune2(){ CANDle.setLEDs(255, 255, 0,0,8,8); CANDle.setLEDs(255, 255, 0,0,24,8);