From efd3f23fa24f4e6d8211f4e7c0af7cd0cd6f81a3 Mon Sep 17 00:00:00 2001 From: Samuel <2540638@etudiant.cegepvicto.com> Date: Fri, 12 Dec 2025 11:08:36 -0500 Subject: [PATCH 1/2] changement --- simgui-ds.json | 5 +++++ src/main/java/frc/robot/subsystems/Bougie.java | 2 +- .../frc/robot/subsystems/CommandSwerveDrivetrain.java | 10 +--------- 3 files changed, 7 insertions(+), 10 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 4a63cc1..1ba71ce 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "Joysticks": { + "window": { + "visible": false + } + }, "System Joysticks": { "window": { "enabled": false diff --git a/src/main/java/frc/robot/subsystems/Bougie.java b/src/main/java/frc/robot/subsystems/Bougie.java index bae7785..7ddda90 100644 --- a/src/main/java/frc/robot/subsystems/Bougie.java +++ b/src/main/java/frc/robot/subsystems/Bougie.java @@ -54,7 +54,7 @@ public class Bougie extends SubsystemBase { if(led==15){ x=true; System.out.println("false"); - }} + }} // candle.setLEDs(255, 0, 0,0,24,8); // candle.setLEDs(255, 0, 0,0,40,8); // candle.setLEDs(255, 0, 0,0,56,8); diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index a0428ca..87dfe30 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -11,7 +11,6 @@ import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; @@ -34,7 +33,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private static final double kSimLoopPeriod = 0.005; // 5 ms private Notifier m_simNotifier = null; private double m_lastSimTime; - private Pigeon2 pigeon2 = new Pigeon2(13); /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ @@ -42,17 +40,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su /* Keep track if we've ever applied the operator perspective before or not */ private boolean m_hasAppliedOperatorPerspective = false; private SwerveDriveOdometry odometry; - private SwerveDrivePoseEstimator poseEstimator; private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - private void configureAutoBuilder() { try { - var config = RobotConfig.fromGUISettings(); + RobotConfig config = RobotConfig.fromGUISettings(); AutoBuilder.configure( () -> getState().Pose, // Supplier of current robot pose From 9fa562450473e903725752e908c1ae7b17f9fbf7 Mon Sep 17 00:00:00 2001 From: Samuel <2540638@etudiant.cegepvicto.com> Date: Thu, 15 Jan 2026 14:20:13 -0500 Subject: [PATCH 2/2] s'enligner sur sible fixe avec limelight --- .vscode/settings.json | 3 +- .../robot/commands/Limelight/AprilTag3.java | 36 ++++++++++++------- .../java/frc/robot/subsystems/Limelight3.java | 11 ++++++ 3 files changed, 37 insertions(+), 13 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 612cdd0..1745ba0 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -56,5 +56,6 @@ "edu.wpi.first.math.proto.*", "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", - ] + ], + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java index 22d141e..5f7b994 100644 --- a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java +++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java @@ -54,30 +54,42 @@ public class AprilTag3 extends Command { public void execute() { double[] BotPose = new double[6]; Optional alliance = DriverStation.getAlliance(); + double y2 = 4; + double x2 = 0; + double angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); + if(angle >180){ + angle =- angle * 2; + } if(alliance.get() == DriverStation.Alliance.Red){ BotPose = limelight3.getBotPoseRed(); + x2 =11.9; } else{ BotPose = limelight3.getBotPoseBlue(); + x2 = 4.1; } double botx = BotPose[0]; double boty = BotPose[1]; double tx = limelight3.getTx(); double tagId = limelight3.getTId(); if(limelight3.getV() == true){ - if(tagId ==10){ - drivetrain.setControl(drive. - withRotationalRate(tx/20). - withVelocityX((botx-5.81)*2). - withVelocityY((boty-4)*4)); - } - else if(tagId ==7){ - drivetrain.setControl(drive. - withRotationalRate(tx/20). - withVelocityX(2-botx). - withVelocityY(2-boty)); + drivetrain.setControl(drive. + withRotationalRate(limelight3.Calcule(botx,x2,boty,y2,angle)). + withVelocityX(x.getAsDouble()). + withVelocityY(y.getAsDouble())); + // if(tagId ==10){ + // drivetrain.setControl(drive. + // withRotationalRate(tx/20). + // withVelocityX((botx-5.81)*2). + // withVelocityY((boty-4)*4)); + // } + // else if(tagId ==7){ + // drivetrain.setControl(drive. + // withRotationalRate(tx/20). + // withVelocityX(2-botx). + // withVelocityY(2-boty)); - } + // } } else{ drivetrain.setControl(drive. diff --git a/src/main/java/frc/robot/subsystems/Limelight3.java b/src/main/java/frc/robot/subsystems/Limelight3.java index 17517b6..0ae293f 100644 --- a/src/main/java/frc/robot/subsystems/Limelight3.java +++ b/src/main/java/frc/robot/subsystems/Limelight3.java @@ -69,6 +69,17 @@ public class Limelight3 extends SubsystemBase { public void Forme(){ pipeline.setNumber(0); } + public double Calcule(double x1, double x2, double y1, double y2, double angle) + { + if (x1 > 4) + { + return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) - angle)/90; + } + else + { + return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) + angle) * -1/90; + } + } @Override public void periodic() { // This method will be called once per scheduler run