From 6c86b2ed5c93d951b7294a11e33708dea7abd294 Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Thu, 2 Apr 2026 20:34:53 -0400 Subject: [PATCH] grimpe mode auto --- .../pathplanner/autos/DepotTirerMur.auto | 6 ---- .../deploy/pathplanner/paths/GrimperMur.path | 8 ++--- .../java/frc/robot/commands/Limelighter.java | 30 ++++++++++--------- .../commands/ModeAuto/BougerDroiteAuto.java | 13 +------- .../robot/commands/ModeAuto/GrimperMur.java | 26 ++++++++-------- .../java/frc/robot/subsystems/Grimpeur.java | 4 +-- .../java/frc/robot/subsystems/LimeLight3.java | 4 +-- .../frc/robot/subsystems/Limelight3G.java | 4 +-- 8 files changed, 40 insertions(+), 55 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/DepotTirerMur.auto b/src/main/deploy/pathplanner/autos/DepotTirerMur.auto index d5346e4..ee119da 100644 --- a/src/main/deploy/pathplanner/autos/DepotTirerMur.auto +++ b/src/main/deploy/pathplanner/autos/DepotTirerMur.auto @@ -41,12 +41,6 @@ "pathName": "Tir" } }, - { - "type": "named", - "data": { - "name": "Limelighter" - } - }, { "type": "deadline", "data": { diff --git a/src/main/deploy/pathplanner/paths/GrimperMur.path b/src/main/deploy/pathplanner/paths/GrimperMur.path index 0ad98a0..0907170 100644 --- a/src/main/deploy/pathplanner/paths/GrimperMur.path +++ b/src/main/deploy/pathplanner/paths/GrimperMur.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.0761055634807417, - "y": 4.85660485021398 + "x": 1.0577108433734939, + "y": 5.532096385542169 }, "prevControl": { - "x": 1.503081312410841, - "y": 4.869543509272469 + "x": 1.4846865923035932, + "y": 5.545035044600658 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/commands/Limelighter.java b/src/main/java/frc/robot/commands/Limelighter.java index 194e0cc..4ad8e8b 100644 --- a/src/main/java/frc/robot/commands/Limelighter.java +++ b/src/main/java/frc/robot/commands/Limelighter.java @@ -7,6 +7,7 @@ package frc.robot.commands; 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; @@ -51,7 +52,6 @@ public class Limelighter extends Command { @Override public void execute() { double[] BotPose = new double[6]; - System.out.println("e"); if (limelight3g.getV()) { BotPose = limelight3g.getBotPoseBlue(); if (!alliance.isPresent()) { @@ -69,19 +69,21 @@ public class Limelighter extends Command { botx = BotPose[0]; boty = BotPose[1]; angle = BotPose[5]; - calcul = limelight3g.Calcule(botx, x, boty, 4, angle); - if(calcul > -5 && calcul < 5){ - drivetrain.setControl( - drive.withRotationalRate(0)); - } - else if(calcul > 5){ - drivetrain.setControl( - drive.withRotationalRate(2)); - } - else if(calcul < -5){ - drivetrain.setControl(drive.withRotationalRate(-2)); - } - // botx = BotPose[1]; + calcul = Math.toRadians(limelight3g.Calcule(boty, 4,botx, x, angle)); + System.out.println(calcul); + drivetrain.setControl(drive.withRotationalRate(MathUtil.clamp(calcul, -3, 3))); + // if(calcul > -5 && calcul < 5){ + // drivetrain.setControl( + // drive.withRotationalRate(0)); + // } + // else if(calcul > 5){ + // drivetrain.setControl( + // drive.withRotationalRate(2)); + // } + // else if(calcul < -5){ + // drivetrain.setControl(drive.withRotationalRate(-2)); + // } + // // botx = BotPose[1]; // boty = BotPose[0]; // angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); // calcul = limelight3g.Calcule(botx, x, boty, 4, angle); diff --git a/src/main/java/frc/robot/commands/ModeAuto/BougerDroiteAuto.java b/src/main/java/frc/robot/commands/ModeAuto/BougerDroiteAuto.java index 72952e7..e3c69f2 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/BougerDroiteAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/BougerDroiteAuto.java @@ -46,23 +46,12 @@ public class BougerDroiteAuto extends Command { // 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"); + if(timer.get() < 1.25){ 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. diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java index c4aab36..4e27543 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java @@ -51,7 +51,7 @@ 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; // } @@ -70,38 +70,38 @@ public class GrimperMur extends Command { if(alliance.get() == Alliance.Red){ pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); System.out.println(pigeonAngle); - y = 2.6; - x = 15.6; + y = 3.6; + x = 14.9; angle = 180; - if(pigeonAngle< 190 && pigeonAngle> 170){ - drivetrain.setControl(drive.withVelocityY(MathUtil.clamp((-(y-boty)*6.5), -2, 2)).withVelocityX(MathUtil.clamp((-(x-botx)*6.5),-2,2))); + if(pigeonAngle> 358 || pigeonAngle< 2){ + drivetrain.setControl(drive.withVelocityY(MathUtil.clamp((-(y-boty)*6.5), -2, 2)).withVelocityX(MathUtil.clamp((-(x-botx)*6.5),-2,2)).withRotationalRate(0)); } else{ if(pigeonAngle>180){ - drivetrain.setControl(drive.withRotationalRate(-1)); + drivetrain.setControl(drive.withVelocityY(0).withVelocityX(0).withRotationalRate(1)); } else{ - drivetrain.setControl(drive.withRotationalRate(1)); + drivetrain.setControl(drive.withVelocityY(0).withVelocityX(0).withRotationalRate(-1)); } } } else{ - pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble()+180; + pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); System.out.println(pigeonAngle); - y = 5.2; - x = 1.11; + y = 4.4; + x = 1.7; angle = 0; - if(pigeonAngle> 358 || pigeonAngle< 2){ + if(pigeonAngle> 182 || pigeonAngle< 178){ 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)); + drivetrain.setControl(drive.withVelocityY(0).withVelocityX(0).withRotationalRate(-1)); System.out.println("x"); } else if(pigeonAngle>180){ System.out.println("e"); - drivetrain.setControl(drive.withRotationalRate(1)); + drivetrain.setControl(drive.withVelocityY(0).withVelocityX(0).withRotationalRate(1)); } } } diff --git a/src/main/java/frc/robot/subsystems/Grimpeur.java b/src/main/java/frc/robot/subsystems/Grimpeur.java index 1ba82c3..9ffd566 100644 --- a/src/main/java/frc/robot/subsystems/Grimpeur.java +++ b/src/main/java/frc/robot/subsystems/Grimpeur.java @@ -21,7 +21,7 @@ public class Grimpeur extends SubsystemBase { SparkMaxConfig slaveConfig = new SparkMaxConfig(); DigitalInput limit = new DigitalInput(0); private GenericEntry EncodeurGrimpeur = - teb.add("Position haut grimpeur", 101).getEntry(); + teb.add("Position haut grimpeur", 100).getEntry(); public void Grimper(double vitesse){ grimpeur1.set(vitesse); grimpeur2.set(vitesse); @@ -36,7 +36,7 @@ public class Grimpeur extends SubsystemBase { return limit.get(); } public double PositionFinal(){ - return EncodeurGrimpeur.getDouble(101); + return EncodeurGrimpeur.getDouble(100); } /** Creates a new Grimpeur. */ public Grimpeur() { diff --git a/src/main/java/frc/robot/subsystems/LimeLight3.java b/src/main/java/frc/robot/subsystems/LimeLight3.java index 5a66220..d7f9659 100644 --- a/src/main/java/frc/robot/subsystems/LimeLight3.java +++ b/src/main/java/frc/robot/subsystems/LimeLight3.java @@ -22,13 +22,13 @@ public class LimeLight3 extends SubsystemBase { } public double[] getBotPoseBlue(){ NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balaie"); - NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpiblue"); + NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpiblue"); double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); return BotPose; } public double[] getBotPoseRed(){ NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balaie"); - NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpired"); + NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpired"); double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); return BotPose; } diff --git a/src/main/java/frc/robot/subsystems/Limelight3G.java b/src/main/java/frc/robot/subsystems/Limelight3G.java index 1303b8f..1973723 100644 --- a/src/main/java/frc/robot/subsystems/Limelight3G.java +++ b/src/main/java/frc/robot/subsystems/Limelight3G.java @@ -69,10 +69,10 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag") // } // } if(y1 > y2){ - return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle; + return Math.toDegrees(Math.atan((x2 - x1) /(y2 - y1))) - angle; } else{ - return -Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle; + return Math.toDegrees(-Math.atan((x2 - x1) / (y2 - y1))) - angle; } } @Override