16 Commits

Author SHA1 Message Date
Samuel
2b5a298433 Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025 2025-12-12 11:09:54 -05:00
Samuel
efd3f23fa2 changement 2025-12-12 11:08:36 -05:00
Antoine PerreaultE
7032b95c60 LE MODE AUTO MARCHE 2025-12-10 19:01:01 -05:00
Antoine PerreaultE
fa30757990 auto I MARCHE(pas) 2025-12-10 18:31:12 -05:00
Antoine PerreaultE
5e9f2f5244 meillieur subsytem swerve (pas tent) 2025-12-08 19:23:02 -05:00
Antoine PerreaultE
0f9a71c375 mode auto dans subsystem fini 2025-12-08 18:29:44 -05:00
Antoine PerreaultE
cf2580ca85 Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025 2025-12-03 17:58:01 -05:00
Antoine PerreaultE
92c1c59599 2025-12-03 17:57:22 -05:00
Samuel
ca694c12d8 Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025 2025-12-01 18:59:05 -05:00
Samuel
b0272a5640 path palnner peut-etre 2025-12-01 18:59:04 -05:00
Antoine PerreaultE
2f147d5edc modifier le path pour les tests 2025-11-26 17:58:22 -05:00
Antoine PerreaultE
9911ae781a Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025 2025-11-24 18:36:50 -05:00
Antoine PerreaultE
1bf8f02db2 s'enligner avec un apriltag fini 2025-11-24 18:36:30 -05:00
Samuel
9e16c18d31 botz = boty 2025-11-24 18:34:29 -05:00
Samuel
68834d11d0 Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025 2025-11-24 18:33:55 -05:00
Samuel
6d3b467a86 path planner est de retour 2025-11-24 18:33:53 -05:00
12 changed files with 216 additions and 122 deletions

View File

@@ -1,4 +1,9 @@
{ {
"Joysticks": {
"window": {
"visible": false
}
},
"System Joysticks": { "System Joysticks": {
"window": { "window": {
"enabled": false "enabled": false

File diff suppressed because one or more lines are too long

View File

@@ -11,7 +11,7 @@
"x": 4.699180327868851, "x": 4.699180327868851,
"y": 2.1728995901639334 "y": 2.1728995901639334
}, },
"isLocked": false, "isLocked": true,
"linkedName": null "linkedName": null
}, },
{ {
@@ -27,7 +27,7 @@
"x": 2.853313965378558, "x": 2.853313965378558,
"y": 1.5363479888253109 "y": 1.5363479888253109
}, },
"isLocked": false, "isLocked": true,
"linkedName": null "linkedName": null
}, },
{ {
@@ -40,7 +40,7 @@
"y": 1.3902362024139756 "y": 1.3902362024139756
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": true,
"linkedName": null "linkedName": null
} }
], ],

View File

@@ -3,25 +3,25 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 7.564241803278687, "x": 16.243,
"y": 3.899129098360655 "y": 6.55025
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 6.506996649367605, "x": 15.354412756981612,
"y": 3.898163120386143 "y": 6.548572497113415
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 6.269569672131147, "x": 1.3552499999999998,
"y": 3.899129098360655 "y": 6.55025
}, },
"prevControl": { "prevControl": {
"x": 7.259410179761819, "x": 2.306674365666888,
"y": 3.9002985318003183 "y": 6.526815183428469
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -34,7 +34,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 1.0,
"maxAngularVelocity": 0.1, "maxAngularVelocity": 0.1,
"maxAngularAcceleration": 0.1, "maxAngularAcceleration": 0.1,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,

View File

@@ -9,7 +9,7 @@
"defaultMaxAngVel": 540.0, "defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 720.0, "defaultMaxAngAccel": 720.0,
"defaultNominalVoltage": 12.0, "defaultNominalVoltage": 12.0,
"robotMass": 45.3592, "robotMass": 45.359,
"robotMOI": 6.883, "robotMOI": 6.883,
"robotTrackwidth": 0.546, "robotTrackwidth": 0.546,
"driveWheelRadius": 0.048, "driveWheelRadius": 0.048,

View File

@@ -10,21 +10,21 @@ import java.util.Map;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.Pigeon2;
import com.ctre.phoenix6.swerve.SwerveRequest; import com.ctre.phoenix6.swerve.SwerveRequest;
import com.pathplanner.lib.auto.AutoBuilder;
import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; 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.Command;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.TunerConstants.TunerConstants; import frc.robot.TunerConstants.TunerConstants;
import frc.robot.commands.RainBow;
import frc.robot.commands.reset; import frc.robot.commands.reset;
import frc.robot.commands.Elevateur.Depart; import frc.robot.commands.Elevateur.Depart;
import frc.robot.commands.Elevateur.ElevateurManuel; import frc.robot.commands.Elevateur.ElevateurManuel;
@@ -59,6 +59,7 @@ import frc.robot.commands.Elevateur.balonL2;
import frc.robot.commands.Elevateur.balonL3; import frc.robot.commands.Elevateur.balonL3;
public class RobotContainer { public class RobotContainer {
private final SendableChooser<Command> autoChooser;
ShuffleboardLayout layoutauto = Shuffleboard.getTab("teb").getLayout("auto", BuiltInLayouts.kList) ShuffleboardLayout layoutauto = Shuffleboard.getTab("teb").getLayout("auto", BuiltInLayouts.kList)
.withSize(2, 2).withProperties(Map.of("Label position", "LEFT")); .withSize(2, 2).withProperties(Map.of("Label position", "LEFT"));
GenericEntry L1 = layoutauto.add("choix L1",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry(); GenericEntry L1 = layoutauto.add("choix L1",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
@@ -91,16 +92,16 @@ public class RobotContainer {
Bougie bougie = new Bougie(); Bougie bougie = new Bougie();
Limelight3G limelight3g = new Limelight3G(); Limelight3G limelight3g = new Limelight3G();
Limelight3 limelight3 = new Limelight3(); Limelight3 limelight3 = new Limelight3();
Pose2d pose = new Pose2d();
Requin requin = new Requin(); Requin requin = new Requin();
CorailAspir corailAspir = new CorailAspir(pince, bougie); CorailAspir corailAspir = new CorailAspir(pince, bougie);
public RobotContainer() { public RobotContainer() {
autoChooser = AutoBuilder.buildAutoChooser();
CameraServer.startAutomaticCapture(); CameraServer.startAutomaticCapture();
SmartDashboard.putData("Auto Chooser", autoChooser);
configureBindings(); configureBindings();
} }
private void configureBindings() { private void configureBindings() {
drivetrain.registerTelemetry(logger::telemeterize);
drivetrain.setDefaultCommand( drivetrain.setDefaultCommand(
// Drivetrain will execute this command periodically // Drivetrain will execute this command periodically
drivetrain.applyRequest(() -> drivetrain.applyRequest(() ->
@@ -115,7 +116,7 @@ public class RobotContainer {
manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie)); manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie));
manette1.rightBumper().toggleOnTrue(new StationPince(pince, elevateur,bougie)); manette1.rightBumper().toggleOnTrue(new StationPince(pince, elevateur,bougie));
manette1.povLeft().whileTrue(new AlgueExpire(pince, 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.povRight().whileTrue(new CoralExpire(pince, bougie));
manette1.leftTrigger().whileTrue(new DepartPince(pince)); manette1.leftTrigger().whileTrue(new DepartPince(pince));
manette1.povDown().whileTrue(new Algue_inspire(pince,bougie)); manette1.povDown().whileTrue(new Algue_inspire(pince,bougie));
@@ -139,8 +140,8 @@ public class RobotContainer {
manette2.x().whileTrue(new ExpireCorail(requin, bougie)); manette2.x().whileTrue(new ExpireCorail(requin, bougie));
//limelight //limelight
manette2.a().whileTrue(new Forme3(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)); // manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain, manette1::getLeftX, manette1::getLeftY));
//Pince manuel //Pince manuel
pince.setDefaultCommand(new RunCommand(()->{ pince.setDefaultCommand(new RunCommand(()->{
@@ -154,37 +155,39 @@ public class RobotContainer {
//Reset encodeur //Reset encodeur
manette2.start().whileTrue(new reset(elevateur, pince, requin)); manette2.start().whileTrue(new reset(elevateur, pince, requin));
drivetrain.registerTelemetry(logger::telemeterize);
} }
public Command getAutonomousCommand() { public Command getAutonomousCommand() {
return new SequentialCommandGroup( return autoChooser.getSelected();
drivetrain.applyRequest(()-> // return new SequentialCommandGroup(
drive.withVelocityX(0.1*MaxSpeed) // drivetrain.applyRequest(()->
.withVelocityY(0) // drive.withVelocityX(0.1*MaxSpeed)
.withRotationalRate(0)).unless(()->!ReculerR.getBoolean(true)).withTimeout(3.5), // .withVelocityY(0)
drivetrain.applyRequest(()-> // .withRotationalRate(0)).unless(()->!ReculerR.getBoolean(true)).withTimeout(3.5),
drive.withVelocityX(-0.1*MaxSpeed) // drivetrain.applyRequest(()->
.withVelocityY(0) // drive.withVelocityX(-0.1*MaxSpeed)
.withRotationalRate(0)).unless(()->!ReculerB.getBoolean(true)).withTimeout(3.5), // .withVelocityY(0)
drivetrain.applyRequest(()-> // .withRotationalRate(0)).unless(()->!ReculerB.getBoolean(true)).withTimeout(3.5),
drive.withVelocityX(-0.1*MaxSpeed) // drivetrain.applyRequest(()->
.withVelocityY(0) // drive.withVelocityX(-0.1*MaxSpeed)
.withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(2.35), // .withVelocityY(0)
drivetrain.applyRequest(()-> // .withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(2.35),
drive.withVelocityX(0.1*MaxSpeed) // drivetrain.applyRequest(()->
.withVelocityY(0) // drive.withVelocityX(0.1*MaxSpeed)
.withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerR.getBoolean(true)).withTimeout(3), // .withVelocityY(0)
drivetrain.applyRequest(()-> // .withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerR.getBoolean(true)).withTimeout(3),
drive.withVelocityX(-0.1*MaxSpeed) // drivetrain.applyRequest(()->
.withVelocityY(0) // drive.withVelocityX(-0.1*MaxSpeed)
.withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerB.getBoolean(true)).withTimeout(3), // .withVelocityY(0)
drivetrain.applyRequest(()-> // .withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerB.getBoolean(true)).withTimeout(3),
drive.withVelocityX(0) // drivetrain.applyRequest(()->
.withVelocityY(0) // drive.withVelocityX(0)
.withRotationalRate(0)).withTimeout(0.1), // .withVelocityY(0)
new L4(elevateur, pince).unless(()->!L4.getBoolean(true)).withTimeout(4), // .withRotationalRate(0)).withTimeout(0.1),
new L1Requin(requin, bougie).unless(()-> !L1.getBoolean(true)).withTimeout(2), // new L4(elevateur, pince).unless(()->!L4.getBoolean(true)).withTimeout(4),
new ExpireCorail(requin, bougie).unless(()->!L1.getBoolean(true)).withTimeout(2), // new L1Requin(requin, bougie).unless(()-> !L1.getBoolean(true)).withTimeout(2),
new RainBow(bougie)); // new ExpireCorail(requin, bougie).unless(()->!L1.getBoolean(true)).withTimeout(2),
} // new RainBow(bougie));
}
} }

View File

@@ -74,8 +74,9 @@ public class AprilTag3 extends Command {
else if(tagId ==7){ else if(tagId ==7){
drivetrain.setControl(drive. drivetrain.setControl(drive.
withRotationalRate(tx/20). withRotationalRate(tx/20).
withVelocityX((2-limelight3.derive(botx))). withVelocityX(2-botx).
withVelocityY(2-limelight3.derive(botz))); withVelocityY(2-boty));
} }
} }
else{ else{

View File

@@ -54,7 +54,7 @@ public class Bougie extends SubsystemBase {
if(led==15){ if(led==15){
x=true; x=true;
System.out.println("false"); System.out.println("false");
}} }}
// candle.setLEDs(255, 0, 0,0,24,8); // candle.setLEDs(255, 0, 0,0,24,8);
// candle.setLEDs(255, 0, 0,0,40,8); // candle.setLEDs(255, 0, 0,0,40,8);
// candle.setLEDs(255, 0, 0,0,56,8); // candle.setLEDs(255, 0, 0,0,56,8);

View File

@@ -1,5 +1,6 @@
package frc.robot.subsystems; package frc.robot.subsystems;
import java.util.function.Supplier; import java.util.function.Supplier;
import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
@@ -11,6 +12,8 @@ import com.pathplanner.lib.config.RobotConfig;
import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation2d;
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.N1;
import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation;
@@ -30,26 +33,49 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
private static final double kSimLoopPeriod = 0.005; // 5 ms private static final double kSimLoopPeriod = 0.005; // 5 ms
private Notifier m_simNotifier = null; private Notifier m_simNotifier = null;
private double m_lastSimTime; private double m_lastSimTime;
/* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero;
/* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg;
/* Keep track if we've ever applied the operator perspective before or not */ /* Keep track if we've ever applied the operator perspective before or not */
private boolean m_hasAppliedOperatorPerspective = false; private boolean m_hasAppliedOperatorPerspective = false;
private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); 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. */ /* 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 void configureAutoBuilder() {
private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); try {
private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); RobotConfig config = RobotConfig.fromGUISettings();
AutoBuilder.configure(
// private void configureAutoBuilder() { () -> 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(ChassisSpeeds.discretize(speeds, 0.020))
.withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
),
new PPHolonomicDriveController(
// PID constants for translation
new PIDConstants(10, 0, 0),
// PID constants for rotation
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());
}
// try { // try {
// var config = RobotConfig.fromGUISettings(); // var config = RobotConfig.fromGUISettings();
// AutoBuilder.configure( // AutoBuilder.configure(
// () -> getState().Pose, // Supplier of current robot pose // () -> getState().Pose, // Supplier of current robot pose
// this::resetPose, // Consumer for seeding pose against auto // this::resetPose, // Consumer for seeding pose against auto
// () -> getState().Speeds, // Supplier of current robot speeds // () -> getState().Speeds, // Supplier of current robot speeds
@@ -62,11 +88,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
// ), // ),
// new PPHolonomicDriveController( // new PPHolonomicDriveController(
// // PID constants for translation // // PID constants for translation
// new PIDConstants(63.167, 0, 0.54521), // new PIDConstants(5, 0, 0),
// // // PID constants for rotation // // // PID constants for rotation
// // new PIDConstants(7.9735, 0, 0.038499) // // new PIDConstants(7.9735, 0, 0.038499)
// // PID constants for rotation // // PID constants for rotation
// new PIDConstants(43.502,0,2.7353) // new PIDConstants(5,0,0)
// ), // ),
// config, // config,
// // Assume the path needs to be flipped for Red vs Blue, this is normally the case // // Assume the path needs to be flipped for Red vs Blue, this is normally the case
@@ -77,11 +103,24 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
// } catch (Exception ex) { // } catch (Exception ex) {
// DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); // DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
// } // }
// PPHolonomicDriveController.overrideRotationFeedback(()->{ // PPHolonomicDriveController.overrideRotationFeedback(()->{
// return 0; // 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. * Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p> * <p>
@@ -100,9 +139,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
if (Utils.isSimulation()) { if (Utils.isSimulation()) {
startSimThread(); startSimThread();
} }
//configureAutoBuilder(); configureAutoBuilder();
} }
/** /**
* Constructs a CTRE SwerveDrivetrain using the specified constants. * Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p> * <p>
@@ -125,7 +163,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
if (Utils.isSimulation()) { if (Utils.isSimulation()) {
startSimThread(); startSimThread();
} }
// configureAutoBuilder(); configureAutoBuilder();
} }
/** /**
@@ -158,7 +196,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
if (Utils.isSimulation()) { if (Utils.isSimulation()) {
startSimThread(); startSimThread();
} }
//configureAutoBuilder(); configureAutoBuilder();
} }

View File

@@ -22,19 +22,6 @@ public class Limelight3 extends SubsystemBase {
PortForwarder.add(port, "limelight.local", port); PortForwarder.add(port, "limelight.local", port);
} }
} }
double previousValue = 0;
double previousTime = Timer.getFPGATimestamp();
public double derive(double currentValue) {
double previousValue = 0;
double previousTime = Timer.getFPGATimestamp();
double now = Timer.getFPGATimestamp();
double dt = now - previousTime;
double derivative = (currentValue - previousValue) / dt;
previousValue = currentValue;
previousTime = now;
return derivative;
}
public double[] getTargetPose(){ public double[] getTargetPose(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon"); NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
NetworkTableEntry targetPoseEntry = limelightTable.getEntry("targetpose_cameraspace"); NetworkTableEntry targetPoseEntry = limelightTable.getEntry("targetpose_cameraspace");

View File

@@ -1,7 +1,7 @@
{ {
"fileName": "PathplannerLib-2025.2.3.json", "fileName": "PathplannerLib-2025.2.6.json",
"name": "PathplannerLib", "name": "PathplannerLib",
"version": "2025.2.3", "version": "2025.2.6",
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
"frcYear": "2025", "frcYear": "2025",
"mavenUrls": [ "mavenUrls": [
@@ -12,7 +12,7 @@
{ {
"groupId": "com.pathplanner.lib", "groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-java", "artifactId": "PathplannerLib-java",
"version": "2025.2.3" "version": "2025.2.6"
} }
], ],
"jniDependencies": [], "jniDependencies": [],
@@ -20,7 +20,7 @@
{ {
"groupId": "com.pathplanner.lib", "groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-cpp", "artifactId": "PathplannerLib-cpp",
"version": "2025.2.3", "version": "2025.2.6",
"libName": "PathplannerLib", "libName": "PathplannerLib",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": false, "sharedLibrary": false,

View File

@@ -1,7 +1,7 @@
{ {
"fileName": "Phoenix6-25.2.2.json", "fileName": "Phoenix6-frc2025-latest.json",
"name": "CTRE-Phoenix (v6)", "name": "CTRE-Phoenix (v6)",
"version": "25.2.2", "version": "25.4.0",
"frcYear": "2025", "frcYear": "2025",
"uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
"mavenUrls": [ "mavenUrls": [
@@ -19,14 +19,14 @@
{ {
"groupId": "com.ctre.phoenix6", "groupId": "com.ctre.phoenix6",
"artifactId": "wpiapi-java", "artifactId": "wpiapi-java",
"version": "25.2.2" "version": "25.4.0"
} }
], ],
"jniDependencies": [ "jniDependencies": [
{ {
"groupId": "com.ctre.phoenix6", "groupId": "com.ctre.phoenix6",
"artifactId": "api-cpp", "artifactId": "api-cpp",
"version": "25.2.2", "version": "25.4.0",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -40,7 +40,7 @@
{ {
"groupId": "com.ctre.phoenix6", "groupId": "com.ctre.phoenix6",
"artifactId": "tools", "artifactId": "tools",
"version": "25.2.2", "version": "25.4.0",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -54,7 +54,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "api-cpp-sim", "artifactId": "api-cpp-sim",
"version": "25.2.2", "version": "25.4.0",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -68,7 +68,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "tools-sim", "artifactId": "tools-sim",
"version": "25.2.2", "version": "25.4.0",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -82,7 +82,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simTalonSRX", "artifactId": "simTalonSRX",
"version": "25.2.2", "version": "25.4.0",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -96,7 +96,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simVictorSPX", "artifactId": "simVictorSPX",
"version": "25.2.2", "version": "25.4.0",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -110,7 +110,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simPigeonIMU", "artifactId": "simPigeonIMU",
"version": "25.2.2", "version": "25.4.0",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -124,7 +124,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simCANCoder", "artifactId": "simCANCoder",
"version": "25.2.2", "version": "25.4.0",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -138,7 +138,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFX", "artifactId": "simProTalonFX",
"version": "25.2.2", "version": "25.4.0",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -152,7 +152,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFXS", "artifactId": "simProTalonFXS",
"version": "25.2.2", "version": "25.4.0",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -166,7 +166,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANcoder", "artifactId": "simProCANcoder",
"version": "25.2.2", "version": "25.4.0",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -180,7 +180,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProPigeon2", "artifactId": "simProPigeon2",
"version": "25.2.2", "version": "25.4.0",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -194,7 +194,35 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANrange", "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, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -210,7 +238,7 @@
{ {
"groupId": "com.ctre.phoenix6", "groupId": "com.ctre.phoenix6",
"artifactId": "wpiapi-cpp", "artifactId": "wpiapi-cpp",
"version": "25.2.2", "version": "25.4.0",
"libName": "CTRE_Phoenix6_WPI", "libName": "CTRE_Phoenix6_WPI",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -226,7 +254,7 @@
{ {
"groupId": "com.ctre.phoenix6", "groupId": "com.ctre.phoenix6",
"artifactId": "tools", "artifactId": "tools",
"version": "25.2.2", "version": "25.4.0",
"libName": "CTRE_PhoenixTools", "libName": "CTRE_PhoenixTools",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -242,7 +270,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "wpiapi-cpp-sim", "artifactId": "wpiapi-cpp-sim",
"version": "25.2.2", "version": "25.4.0",
"libName": "CTRE_Phoenix6_WPISim", "libName": "CTRE_Phoenix6_WPISim",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -258,7 +286,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "tools-sim", "artifactId": "tools-sim",
"version": "25.2.2", "version": "25.4.0",
"libName": "CTRE_PhoenixTools_Sim", "libName": "CTRE_PhoenixTools_Sim",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -274,7 +302,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simTalonSRX", "artifactId": "simTalonSRX",
"version": "25.2.2", "version": "25.4.0",
"libName": "CTRE_SimTalonSRX", "libName": "CTRE_SimTalonSRX",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -290,7 +318,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simVictorSPX", "artifactId": "simVictorSPX",
"version": "25.2.2", "version": "25.4.0",
"libName": "CTRE_SimVictorSPX", "libName": "CTRE_SimVictorSPX",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -306,7 +334,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simPigeonIMU", "artifactId": "simPigeonIMU",
"version": "25.2.2", "version": "25.4.0",
"libName": "CTRE_SimPigeonIMU", "libName": "CTRE_SimPigeonIMU",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -322,7 +350,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simCANCoder", "artifactId": "simCANCoder",
"version": "25.2.2", "version": "25.4.0",
"libName": "CTRE_SimCANCoder", "libName": "CTRE_SimCANCoder",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -338,7 +366,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFX", "artifactId": "simProTalonFX",
"version": "25.2.2", "version": "25.4.0",
"libName": "CTRE_SimProTalonFX", "libName": "CTRE_SimProTalonFX",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -354,7 +382,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFXS", "artifactId": "simProTalonFXS",
"version": "25.2.2", "version": "25.4.0",
"libName": "CTRE_SimProTalonFXS", "libName": "CTRE_SimProTalonFXS",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -370,7 +398,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANcoder", "artifactId": "simProCANcoder",
"version": "25.2.2", "version": "25.4.0",
"libName": "CTRE_SimProCANcoder", "libName": "CTRE_SimProCANcoder",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -386,7 +414,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProPigeon2", "artifactId": "simProPigeon2",
"version": "25.2.2", "version": "25.4.0",
"libName": "CTRE_SimProPigeon2", "libName": "CTRE_SimProPigeon2",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -402,7 +430,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANrange", "artifactId": "simProCANrange",
"version": "25.2.2", "version": "25.4.0",
"libName": "CTRE_SimProCANrange", "libName": "CTRE_SimProCANrange",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -414,6 +442,38 @@
"osxuniversal" "osxuniversal"
], ],
"simMode": "swsim" "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"
} }
] ]
} }