From 673a7fcb826b33007826c21c73c0ef1466395684 Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Sat, 28 Mar 2026 20:50:33 -0400 Subject: [PATCH] vgft --- src/main/deploy/pathplanner/paths/Depot.path | 8 ++-- src/main/deploy/pathplanner/paths/Tir.path | 10 ++--- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/commands/Lancer.java | 7 ++-- .../java/frc/robot/commands/Limelighter.java | 3 +- .../robot/commands/ModeAuto/LancerAuto.java | 41 ++++++++++--------- .../commands/ModeAuto/LimelighterAuto.java | 10 ++--- 7 files changed, 43 insertions(+), 38 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Depot.path b/src/main/deploy/pathplanner/paths/Depot.path index 43057b1..664610b 100644 --- a/src/main/deploy/pathplanner/paths/Depot.path +++ b/src/main/deploy/pathplanner/paths/Depot.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.5732667617689007, + "x": 3.6120827389443653, "y": 5.917574893009986 }, "prevControl": null, "nextControl": { - "x": 2.266462196861626, + "x": 2.3052781740370905, "y": 5.904636233951498 }, "isLocked": false, @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 1.1796148359486442, + "x": 0.4938659058487873, "y": 5.917574893009986 }, "prevControl": { - "x": 1.7230385164051354, + "x": 1.0372895863052785, "y": 5.930513552068473 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Tir.path b/src/main/deploy/pathplanner/paths/Tir.path index 2e1e0cb..7a9bc77 100644 --- a/src/main/deploy/pathplanner/paths/Tir.path +++ b/src/main/deploy/pathplanner/paths/Tir.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.1796148359486442, - "y": 5.917574893009986 + "x": 0.5197432239657629, + "y": 5.930513552068473 }, "prevControl": null, "nextControl": { - "x": 2.3570328102710407, - "y": 5.982268188302426 + "x": 1.6971611982881594, + "y": 5.995206847360913 }, "isLocked": false, "linkedName": null @@ -30,7 +30,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.2179128348660559, + "waypointRelativePos": 0.3240938166311289, "rotationDegrees": 180.0 } ], diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 26599b1..59e696e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -98,7 +98,7 @@ public class RobotContainer { drivetrain.applyRequest(() -> drive.withVelocityY(MathUtil.applyDeadband(-manette.getLeftX()*Math.abs(-manette.getLeftX())*MaxSpeed*0.7, 0.05)) .withVelocityX(MathUtil.applyDeadband(-manette.getLeftY()*Math.abs(-manette.getLeftY())*MaxSpeed*0.7, 0.05)) - .withRotationalRate(MathUtil.applyDeadband(-manette.getRightX()*Math.abs(-manette.getRightX())*MaxAngularRate*0.7, 0.05)) + .withRotationalRate(MathUtil.applyDeadband(-manette.getRightX()*Math.abs(-manette.getRightX())*MaxAngularRate, 0.05)) ) ); //manette 1 diff --git a/src/main/java/frc/robot/commands/Lancer.java b/src/main/java/frc/robot/commands/Lancer.java index 66a39fb..f1df279 100644 --- a/src/main/java/frc/robot/commands/Lancer.java +++ b/src/main/java/frc/robot/commands/Lancer.java @@ -37,7 +37,7 @@ public class Lancer extends Command { // Called when the command is initially scheduled. @Override public void initialize() { - if(limeLight3G.getV()){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()){} pidController = new PIDController(0.0007, 0,0, 0.001); timer.reset(); } @@ -47,7 +47,7 @@ public class Lancer extends Command { public void execute() { double[] BotPose = new double[6]; if(limeLight3G.getV()){ - + if(!alliance.isPresent()){return;} if(alliance.get() == Alliance.Blue){ BotPose = limeLight3G.getBotPoseBlue(); } @@ -56,7 +56,8 @@ public class Lancer extends Command { } botx = BotPose[0]; boty = BotPose[1]; - } + 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); diff --git a/src/main/java/frc/robot/commands/Limelighter.java b/src/main/java/frc/robot/commands/Limelighter.java index 97a882e..dfef133 100644 --- a/src/main/java/frc/robot/commands/Limelighter.java +++ b/src/main/java/frc/robot/commands/Limelighter.java @@ -50,13 +50,14 @@ public class Limelighter extends Command { double[] BotPose = new double[6]; System.out.println("e"); if (limelight3g.getV()) { + if(!alliance.isPresent()){return;} 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/LancerAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java index cc036e0..2205a84 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java @@ -22,7 +22,8 @@ public class LancerAuto extends Command { Limelight3G limelight3g; double botx = 0; double boty = 0; - Optional alliance = DriverStation.getAlliance(); + Optional alliance = DriverStation.getAlliance(); + /** Creates a new LancerAuto. */ public LancerAuto(Lanceur lanceur, Limelight3G limelight3g) { this.lanceur = lanceur; @@ -35,41 +36,43 @@ public class LancerAuto extends Command { // Called when the command is initially scheduled. @Override public void initialize() { - pidController = new PIDController(0.0007, 0,0, 0.001); + pidController = new PIDController(0.0007, 0, 0, 0.001); + timer.reset(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { double[] BotPose = new double[6]; - if(limelight3g.getV()){ - if(alliance.isPresent()){ - if(alliance.get() == Alliance.Blue){ - BotPose = limelight3g.getBotPoseBlue(); + if (limelight3g.getV()) { + if (!alliance.isPresent()) { + return; } - else{ + if (alliance.get() == Alliance.Blue) { + BotPose = limelight3g.getBotPoseBlue(); + } else { BotPose = limelight3g.getBotPoseRed(); - } - } + } + botx = BotPose[0]; boty = BotPose[1]; - } + } double vitesse = 0.5; - if(limelight3g.getV()){ - vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250; - System.out.println(vitesse); + if (limelight3g.getV()) { + vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594 - botx), 2) + Math.pow(Math.abs(4.034536 - boty), 2)))) + + 2250; + System.out.println("lancer"); - double output = pidController.calculate(lanceur.Vitesse(),vitesse); + double output = pidController.calculate(lanceur.Vitesse(), vitesse); lanceur.Lancer(output); - if(lanceur.Vitesse() >= vitesse-800){ + if (lanceur.Vitesse() >= vitesse - 800) { timer.start(); - if(timer.get() >1){ + if (timer.get() > 1) { lanceur.Demeler(1); } - + } } } - } // Called once the command ends or is interrupted. @Override @@ -84,6 +87,6 @@ public class LancerAuto extends Command { @Override public boolean isFinished() { return timer.get() > 4; - //return false; + // return false; } } diff --git a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java index 02351f6..5e6dbc9 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java @@ -53,16 +53,15 @@ public class LimelighterAuto extends Command { @Override public void execute() { double[] BotPose = new double[6]; - System.out.println("a"); if (limelight3g.getV()) { - if(alliance.isPresent()){ + if(!alliance.isPresent()){return;} if(alliance.get() == Alliance.Blue){ BotPose = limelight3g.getBotPoseBlue(); } else{ BotPose = limelight3g.getBotPoseRed(); } - } + botx = BotPose[1]; boty = BotPose[0]; angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); @@ -72,8 +71,9 @@ public class LimelighterAuto extends Command { calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 5; drivetrain.setControl( drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); + System.out.println("Limelighter"); System.out.println(calcul); - if (calcul < 0.1 && calcul > -0.1) { + if (calcul < 0.3 && calcul > -0.3) { drivetrain.setControl(drive.withRotationalRate(0)); } } @@ -92,7 +92,7 @@ public class LimelighterAuto extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return calcul < 0.1 && calcul > -0.1; + return calcul < 0.3 && calcul > -0.3; } }