diff --git a/src/main/deploy/pathplanner/paths/BlueBasChercher.path b/src/main/deploy/pathplanner/paths/BlueBasChercher.path index 7a2e9c8..be8a2f6 100644 --- a/src/main/deploy/pathplanner/paths/BlueBasChercher.path +++ b/src/main/deploy/pathplanner/paths/BlueBasChercher.path @@ -11,7 +11,7 @@ "x": 4.699180327868851, "y": 2.1728995901639334 }, - "isLocked": false, + "isLocked": true, "linkedName": null }, { @@ -27,7 +27,7 @@ "x": 2.853313965378558, "y": 1.5363479888253109 }, - "isLocked": false, + "isLocked": true, "linkedName": null }, { @@ -40,7 +40,7 @@ "y": 1.3902362024139756 }, "nextControl": null, - "isLocked": false, + "isLocked": true, "linkedName": null } ], diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 601f47e..d23e318 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 16.243499999999997, + "x": 16.243, "y": 6.55025 }, "prevControl": null, "nextControl": { - "x": 15.18625440479427, - "y": 6.55025 + "x": 15.354412756981612, + "y": 6.548572497113415 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 6.55025 }, "prevControl": { - "x": 2.34525, - "y": 6.55025 + "x": 2.306674365666888, + "y": 6.526815183428469 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a5477ff..d9a83c0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,7 +14,6 @@ import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; @@ -23,12 +22,9 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.TunerConstants.TunerConstants; -import frc.robot.commands.RainBow; import frc.robot.commands.reset; import frc.robot.commands.Elevateur.Depart; import frc.robot.commands.Elevateur.ElevateurManuel; @@ -96,7 +92,6 @@ public class RobotContainer { Bougie bougie = new Bougie(); Limelight3G limelight3g = new Limelight3G(); Limelight3 limelight3 = new Limelight3(); - Pose2d pose = new Pose2d(); Requin requin = new Requin(); CorailAspir corailAspir = new CorailAspir(pince, bougie); public RobotContainer() { @@ -107,7 +102,6 @@ public class RobotContainer { } private void configureBindings() { - drivetrain.registerTelemetry(logger::telemeterize); drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically drivetrain.applyRequest(() -> @@ -122,7 +116,7 @@ public class RobotContainer { manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie)); manette1.rightBumper().toggleOnTrue(new StationPince(pince, elevateur,bougie)); manette1.povLeft().whileTrue(new AlgueExpire(pince, bougie)); - manette1.leftBumper().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY)); + // manette1.leftBumper().whileTrue(new AprilTag3G(limelight3g,drivetrain1,manette1::getLeftX,manette1::getLeftY)); manette1.povRight().whileTrue(new CoralExpire(pince, bougie)); manette1.leftTrigger().whileTrue(new DepartPince(pince)); manette1.povDown().whileTrue(new Algue_inspire(pince,bougie)); @@ -146,8 +140,8 @@ public class RobotContainer { manette2.x().whileTrue(new ExpireCorail(requin, bougie)); //limelight - manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); - manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain, manette1::getLeftX, manette1::getLeftY)); + // manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); + // manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain, manette1::getLeftX, manette1::getLeftY)); //Pince manuel pince.setDefaultCommand(new RunCommand(()->{ @@ -161,6 +155,7 @@ public class RobotContainer { //Reset encodeur manette2.start().whileTrue(new reset(elevateur, pince, requin)); + drivetrain.registerTelemetry(logger::telemeterize); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java index 81d297c..22d141e 100644 --- a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java +++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java @@ -76,6 +76,7 @@ public class AprilTag3 extends Command { withRotationalRate(tx/20). withVelocityX(2-botx). withVelocityY(2-boty)); + } } else{ diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 87dfe30..baa52e7 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -1,8 +1,8 @@ package frc.robot.subsystems; + import java.util.function.Supplier; import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveRequest; @@ -11,9 +11,9 @@ 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.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.geometry.Translation2d; +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.wpilibj.DriverStation; @@ -39,46 +39,88 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; /* Keep track if we've ever applied the operator perspective before or not */ private boolean m_hasAppliedOperatorPerspective = false; - private SwerveDriveOdometry odometry; private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); - +// Position des modules par rapport au centre du robot +Translation2d frontLeftLocation = new Translation2d(+0.3, +0.3); +Translation2d frontRightLocation = new Translation2d(+0.3, -0.3); +Translation2d backLeftLocation = new Translation2d(-0.3, +0.3); +Translation2d backRightLocation = new Translation2d(-0.3, -0.3); + /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ private void configureAutoBuilder() { try { RobotConfig config = RobotConfig.fromGUISettings(); AutoBuilder.configure( - () -> getState().Pose, // Supplier of current robot pose - this::resetOdometry, // Consumer for seeding pose against auto + this::resetPose, // Consumer for seeding pose against auto () -> getState().Speeds, // Supplier of current robot speeds - // Consumer of ChassisSpeeds and feedforwards to drive the robot (speeds, feedforwards) -> setControl( - m_pathApplyRobotSpeeds.withSpeeds(speeds) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesYNewtons()) + m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020)) + .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) ), new PPHolonomicDriveController( // PID constants for translation - new PIDConstants(5, 0, 0), - // // PID constants for rotation - // new PIDConstants(7.9735, 0, 0.038499) + new PIDConstants(10, 0, 0), // PID constants for rotation - new PIDConstants(5,0,0) + new PIDConstants(7, 0, 0) ), config, // Assume the path needs to be flipped for Red vs Blue, this is normally the case () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, this // Subsystem for requirements - ); } catch (Exception ex) { DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); } - PPHolonomicDriveController.overrideRotationFeedback(()->{ - return 0; - }); - } + // try { + // var config = RobotConfig.fromGUISettings(); + // AutoBuilder.configure( + // () -> getState().Pose, // Supplier of current robot pose + // this::resetPose, // Consumer for seeding pose against auto + // () -> getState().Speeds, // Supplier of current robot speeds + + // // Consumer of ChassisSpeeds and feedforwards to drive the robot + // (speeds, feedforwards) -> setControl( + // m_pathApplyRobotSpeeds.withSpeeds(speeds) + // .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesXNewtons()) + // .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesYNewtons()) + // ), + // new PPHolonomicDriveController( + // // PID constants for translation + // new PIDConstants(5, 0, 0), + // // // PID constants for rotation + // // new PIDConstants(7.9735, 0, 0.038499) + // // PID constants for rotation + // new PIDConstants(5,0,0) + // ), + // config, + // // Assume the path needs to be flipped for Red vs Blue, this is normally the case + // () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + // this // Subsystem for requirements + + // ); + // } catch (Exception ex) { + // DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); + // } + // PPHolonomicDriveController.overrideRotationFeedback(()->{ + // return 0; + // }); + } +// public void driveRobotRelative(ChassisSpeeds speeds) { +// // Convertir robot-relative ChassisSpeeds → SwerveModuleStates +// SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds); + +// // Limitations des roues +// SwerveDriveKinematics.desaturateWheelSpeeds(states, TunerConstants.kSpeedAt12Volts); + +// // Envoi aux modules +// frontLeft.setDesiredState(states[0]); +// frontRight.setDesiredState(states[1]); +// backLeft.setDesiredState(states[2]); +// backRight.setDesiredState(states[3]); +// } /** * Constructs a CTRE SwerveDrivetrain using the specified constants. *

@@ -97,27 +139,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su if (Utils.isSimulation()) { startSimThread(); } - configureAutoBuilder(); - } - // public void resetPose(Pose2d pose) { - // // Reset gyro to match starting rotation - // pigeon2.setYaw(pose.getRotation().getDegrees()); - - // // Reset odometry - // m_odometry.resetPosition( - // pose.getRotation(), - // getState().ModulePositions, - // pose - // ); - //} - public void resetOdometry(Pose2d pose) { - odometry = new SwerveDriveOdometry(getKinematics(), kBlueAlliancePerspectiveRotation, getState().ModulePositions); - odometry.resetPosition( - getPigeon2().getRotation2d(), - getState().ModulePositions, - pose - ); - this.getPigeon2().setYaw(pose.getRotation().getDegrees()); + configureAutoBuilder(); } /** * Constructs a CTRE SwerveDrivetrain using the specified constants. diff --git a/vendordeps/PathplannerLib-2025.2.3.json b/vendordeps/PathplannerLib-2025.2.6.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.3.json rename to vendordeps/PathplannerLib-2025.2.6.json index 9151ce4..95ba203 100644 --- a/vendordeps/PathplannerLib-2025.2.3.json +++ b/vendordeps/PathplannerLib-2025.2.6.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.3.json", + "fileName": "PathplannerLib-2025.2.6.json", "name": "PathplannerLib", - "version": "2025.2.3", + "version": "2025.2.6", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.3" + "version": "2025.2.6" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.3", + "version": "2025.2.6", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-25.2.2.json b/vendordeps/Phoenix6-frc2025-latest.json similarity index 80% rename from vendordeps/Phoenix6-25.2.2.json rename to vendordeps/Phoenix6-frc2025-latest.json index d617643..6f40c84 100644 --- a/vendordeps/Phoenix6-25.2.2.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.2.2.json", + "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.2.2", + "version": "25.4.0", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.2" + "version": "25.4.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,35 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.2", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +238,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +254,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +270,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +286,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +302,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +318,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +334,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +350,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +366,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +382,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +398,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +414,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +430,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -414,6 +442,38 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" } ] } \ No newline at end of file