From 799e3e34c3078caa73d6ef1dcad5bc4410a04955 Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Sat, 28 Mar 2026 15:53:53 -0400 Subject: [PATCH] m --- .../robot/commands/ModeAuto/GrimperMur.java | 13 +++--- .../commands/ModeAuto/GrimperReservoir.java | 43 ++++++++++--------- .../commands/ModeAuto/LimelighterAuto.java | 4 +- .../java/frc/robot/subsystems/Grimpeur.java | 4 +- 4 files changed, 33 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java index d69b934..7201f79 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java @@ -55,6 +55,7 @@ public class GrimperMur extends Command { BotPose = limeLight3.getBotPoseBlue(); botx = BotPose[0]; boty = BotPose[1]; + System.out.println(drivetrain.getPigeon2().getYaw().getValueAsDouble()); if(angle < 0){ angle = angle + 360; } @@ -62,15 +63,15 @@ public class GrimperMur extends Command { y = 2.961328; x = 1.11; angle = 0; - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 355 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 5){ + if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){ drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); } else{ if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(-0.5)); + drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI)); } else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(0.5)); + drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI)); } } } @@ -78,15 +79,15 @@ public class GrimperMur extends Command { x = 11.45966; y = 6.959326; angle = 180; - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 175 && drivetrain.getPigeon2().getYaw().getValueAsDouble() < 185){ + if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170 && drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190){ drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); } else{ if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(0.5)); + drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI)); } else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(-0.5)); + drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI)); } } } diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java index 87154ec..ab44170 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java @@ -55,38 +55,39 @@ public class GrimperReservoir extends Command { 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.Blue){ - y = 5.081328; - x = 1.11; - angle = 180; - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() < 185 && drivetrain.getPigeon2().getYaw().getValueAsDouble() > 175){ - drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); - } - else{ - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(0.5)); - } - else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(-0.5)); - } - } - } - else{ + if(alliance.get() == Alliance.Red){ y = 6.959326; x = 13.57966; - angle = 0; - if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 355 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 5){ + angle = 180; + if(drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190 && drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170){ drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); } else{ if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(-0.5)); + drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI)); } else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(0.5)); + drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI)); + } + } + } + else{ + y = 5.081328; + x = 1.11; + angle = 0; + if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){ + drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(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)); } } } diff --git a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java index f4b4bbc..7de1e8a 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java @@ -69,7 +69,7 @@ public class LimelighterAuto extends Command { if(angle >180){ angle -= 360; } - calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 5; + calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 3; drivetrain.setControl( drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); System.out.println(calcul); @@ -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.2 && calcul > -0.2; } } diff --git a/src/main/java/frc/robot/subsystems/Grimpeur.java b/src/main/java/frc/robot/subsystems/Grimpeur.java index 9ffd566..1ba82c3 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", 100).getEntry(); + teb.add("Position haut grimpeur", 101).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(100); + return EncodeurGrimpeur.getDouble(101); } /** Creates a new Grimpeur. */ public Grimpeur() {