sdesharnais/Rebuilt-2026
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'main' of https://git.demerso.net/sdesharnais/Rebuilt-2026

Browse Source
This commit is contained in:
Antoine PerreaultE
2026-03-28 13:21:35 -04:00
parent a92a7a0ea1 ba57959f4d
commit 85da1dcad1
8 changed files with 332 additions and 304 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/main/deploy/pathplanner/paths/TirerSimple.path
View File

@@ -3,12 +3,12 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 3.5473894436519258, "x": 3.602,
"y": 4.015592011412268 "y": 4.015592011412268
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 2.494897544356074, "x": 2.549508100704148,
"y": 4.011425161481168 "y": 4.011425161481168
}, },
"isLocked": false, "isLocked": false,
@@ -16,12 +16,12 @@
}, },
{ {
"anchor": { "anchor": {
"x": 1.7359771754636228, "x": 1.643730569948188,
"y": 4.015592011412268 "y": 4.015592011412268
}, },
"prevControl": { "prevControl": {
"x": 2.627549770741877, "x": 2.5679620034542325,
"y": 4.02839075681212 "y": 4.011502590673576
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,

4
src/main/deploy/pathplanner/settings.json
View File

@@ -16,12 +16,12 @@
"robotMass": 51.673, "robotMass": 51.673,
"robotMOI": 6.883, "robotMOI": 6.883,
"robotTrackwidth": 0.546, "robotTrackwidth": 0.546,
"driveWheelRadius": 0.099, "driveWheelRadius": 0.051,
"driveGearing": 6.2, "driveGearing": 6.2,
"maxDriveSpeed": 9.82, "maxDriveSpeed": 9.82,
"driveMotorType": "krakenX60", "driveMotorType": "krakenX60",
"driveCurrentLimit": 120.0, "driveCurrentLimit": 120.0,
"wheelCOF": 1.1, "wheelCOF": 1.2,
"flModuleX": 0.288925, "flModuleX": 0.288925,
"flModuleY": 0.269875, "flModuleY": 0.269875,
"frModuleX": 0.288925, "frModuleX": 0.288925,

3
src/main/java/frc/robot/RobotContainer.java
View File

@@ -33,6 +33,7 @@ import frc.robot.commands.ModeAuto.AspirerAuto;
import frc.robot.commands.ModeAuto.GrimperMur; import frc.robot.commands.ModeAuto.GrimperMur;
import frc.robot.commands.ModeAuto.GrimperReservoir; import frc.robot.commands.ModeAuto.GrimperReservoir;
import frc.robot.commands.ModeAuto.LancerAuto; import frc.robot.commands.ModeAuto.LancerAuto;
import frc.robot.commands.ModeAuto.LimelighterAuto;
import frc.robot.commands.ModeAuto.RetourMilieuDroite; import frc.robot.commands.ModeAuto.RetourMilieuDroite;
import frc.robot.commands.ModeAuto.RetourMilieuGauche; import frc.robot.commands.ModeAuto.RetourMilieuGauche;
import frc.robot.commands.ModeAuto.TournerVersMur; import frc.robot.commands.ModeAuto.TournerVersMur;
@@ -77,7 +78,7 @@ public class RobotContainer {
NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur, limeLight3G)); NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur, limeLight3G));
NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite(drivetrain,limeLight3G)); NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite(drivetrain,limeLight3G));
NamedCommands.registerCommand("RetourMilieuGauche", new RetourMilieuGauche(drivetrain, limeLight3G)); NamedCommands.registerCommand("RetourMilieuGauche", new RetourMilieuGauche(drivetrain, limeLight3G));
NamedCommands.registerCommand("Limelighter", new Limelighter(limeLight3G,drivetrain)); NamedCommands.registerCommand("Limelighter", new LimelighterAuto(limeLight3G,drivetrain));
NamedCommands.registerCommand("DescendreBalayeuse", new DescendreBalyeuse(balayeuse)); NamedCommands.registerCommand("DescendreBalayeuse", new DescendreBalyeuse(balayeuse));
NamedCommands.registerCommand("Aspirer", new AspirerAuto(balayeuse)); NamedCommands.registerCommand("Aspirer", new AspirerAuto(balayeuse));
NamedCommands.registerCommand("TournerA180", new TournerVersReservoir(drivetrain)); NamedCommands.registerCommand("TournerA180", new TournerVersReservoir(drivetrain));

24
src/main/java/frc/robot/commands/Lancer.java
View File

@@ -4,7 +4,11 @@
package frc.robot.commands; package frc.robot.commands;
import java.util.Optional;
import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Lanceur; import frc.robot.subsystems.Lanceur;
@@ -16,6 +20,11 @@ public class Lancer extends Command {
private PIDController pidController; private PIDController pidController;
private Limelight3G limeLight3G; private Limelight3G limeLight3G;
private Timer timer; private Timer timer;
double vitesse = 0.5;
double botx = 0;
double boty = 0;
Optional<Alliance> alliance = DriverStation.getAlliance();
/** Creates a new Lancer. */ /** Creates a new Lancer. */
public Lancer(Lanceur lanceur, Limelight3G limeLight3G) { public Lancer(Lanceur lanceur, Limelight3G limeLight3G) {
this.lanceur = lanceur; this.lanceur = lanceur;
@@ -28,6 +37,7 @@ public class Lancer extends Command {
// Called when the command is initially scheduled. // Called when the command is initially scheduled.
@Override @Override
public void initialize() { public void initialize() {
if(limeLight3G.getV()){vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250;}
pidController = new PIDController(0.0007, 0,0, 0.001); pidController = new PIDController(0.0007, 0,0, 0.001);
timer.reset(); timer.reset();
} }
@@ -35,17 +45,19 @@ public class Lancer extends Command {
// Called every time the scheduler runs while the command is scheduled. // Called every time the scheduler runs while the command is scheduled.
@Override @Override
public void execute() { public void execute() {
double botx = 0;
double boty = 0;
double[] BotPose = new double[6]; double[] BotPose = new double[6];
if(limeLight3G.getV()){ if(limeLight3G.getV()){
BotPose = limeLight3G.getBotPoseBlue();
if(alliance.get() == Alliance.Blue){
BotPose = limeLight3G.getBotPoseBlue();
}
else{
BotPose = limeLight3G.getBotPoseRed();
}
botx = BotPose[0]; botx = BotPose[0];
boty = BotPose[1]; boty = BotPose[1];
} }
double vitesse = 0.5;
if(limeLight3G.getV()){ if(limeLight3G.getV()){
vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250;
System.out.println(vitesse); System.out.println(vitesse);
double output = pidController.calculate(lanceur.Vitesse(),vitesse); double output = pidController.calculate(lanceur.Vitesse(),vitesse);

15
src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java
View File

@@ -4,7 +4,11 @@
package frc.robot.commands.ModeAuto; package frc.robot.commands.ModeAuto;
import java.util.Optional;
import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Lanceur; import frc.robot.subsystems.Lanceur;
@@ -18,6 +22,7 @@ public class LancerAuto extends Command {
Limelight3G limelight3g; Limelight3G limelight3g;
double botx = 0; double botx = 0;
double boty = 0; double boty = 0;
Optional<Alliance> alliance = DriverStation.getAlliance();
/** Creates a new LancerAuto. */ /** Creates a new LancerAuto. */
public LancerAuto(Lanceur lanceur, Limelight3G limelight3g) { public LancerAuto(Lanceur lanceur, Limelight3G limelight3g) {
this.lanceur = lanceur; this.lanceur = lanceur;
@@ -38,13 +43,19 @@ public class LancerAuto extends Command {
public void execute() { public void execute() {
double[] BotPose = new double[6]; double[] BotPose = new double[6];
if(limelight3g.getV()){ if(limelight3g.getV()){
BotPose = limelight3g.getBotPoseBlue();
if(alliance.get() == Alliance.Blue){
BotPose = limelight3g.getBotPoseBlue();
}
else{
BotPose = limelight3g.getBotPoseRed();
}
botx = BotPose[0]; botx = BotPose[0];
boty = BotPose[1]; boty = BotPose[1];
} }
double vitesse = 0.5; double vitesse = 0.5;
if(limelight3g.getV()){ if(limelight3g.getV()){
vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250; vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250;
System.out.println(vitesse); System.out.println(vitesse);
double output = pidController.calculate(lanceur.Vitesse(),vitesse); double output = pidController.calculate(lanceur.Vitesse(),vitesse);

5
src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java
View File

@@ -13,7 +13,10 @@ import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.generated.TunerConstants; import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.Limelight3G; import frc.robot.subsystems.Limelight3G;
import static edu.wpi.first.units.Units.*;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import java.util.Optional; import java.util.Optional;

573
src/main/java/frc/robot/generated/TunerConstants.java
View File

@@ -1,286 +1,287 @@
package frc.robot.generated; package frc.robot.generated;
import static edu.wpi.first.units.Units.*; import static edu.wpi.first.units.Units.*;
import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.configs.*; import com.ctre.phoenix6.configs.*;
import com.ctre.phoenix6.hardware.*; import com.ctre.phoenix6.hardware.*;
import com.ctre.phoenix6.signals.*; import com.ctre.phoenix6.signals.*;
import com.ctre.phoenix6.swerve.*; import com.ctre.phoenix6.swerve.*;
import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; import com.ctre.phoenix6.swerve.SwerveModuleConstants.*;
import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Matrix;
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.units.measure.*; import edu.wpi.first.units.measure.*;
import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.CommandSwerveDrivetrain;
// Generated by the 2026 Tuner X Swerve Project Generator // Generated by the 2026 Tuner X Swerve Project Generator
// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
public class TunerConstants { public class TunerConstants {
// Both sets of gains need to be tuned to your individual robot. // Both sets of gains need to be tuned to your individual robot.
// The steer motor uses any SwerveModule.SteerRequestType control request with the // The steer motor uses any SwerveModule.SteerRequestType control request with the
// output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
private static final Slot0Configs steerGains = new Slot0Configs() private static final Slot0Configs steerGains = new Slot0Configs()
.withKP(100).withKI(0).withKD(0.5) .withKP(100).withKI(0).withKD(0.5)
.withKS(0.1).withKV(2.66).withKA(0) .withKS(0.1).withKV(2.66).withKA(0)
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
// When using closed-loop control, the drive motor uses the control // When using closed-loop control, the drive motor uses the control
// output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
private static final Slot0Configs driveGains = new Slot0Configs() private static final Slot0Configs driveGains = new Slot0Configs()
.withKP(0.1).withKI(0).withKD(0) .withKP(0.1).withKI(0).withKD(0)
.withKS(0).withKV(0.124); .withKS(0).withKV(0.124);
// The closed-loop output type to use for the steer motors; // The closed-loop output type to use for the steer motors;
// This affects the PID/FF gains for the steer motors // This affects the PID/FF gains for the steer motors
private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
// The closed-loop output type to use for the drive motors; // The closed-loop output type to use for the drive motors;
// This affects the PID/FF gains for the drive motors // This affects the PID/FF gains for the drive motors
private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage;
// The type of motor used for the drive motor // The type of motor used for the drive motor
private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated;
// The type of motor used for the drive motor // The type of motor used for the drive motor
private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated;
// The remote sensor feedback type to use for the steer motors; // The remote sensor feedback type to use for the steer motors;
// When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote*
private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
// The stator current at which the wheels start to slip; // The stator current at which the wheels start to slip;
// This needs to be tuned to your individual robot // This needs to be tuned to your individual robot
private static final Current kSlipCurrent = Amps.of(120); private static final Current kSlipCurrent = Amps.of(120);
// Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
// Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration()
.withCurrentLimits( .withCurrentLimits(
new CurrentLimitsConfigs() new CurrentLimitsConfigs()
// Swerve azimuth does not require much torque output, so we can set a relatively low // Swerve azimuth does not require much torque output, so we can set a relatively low
// stator current limit to help avoid brownouts without impacting performance. // stator current limit to help avoid brownouts without impacting performance.
.withStatorCurrentLimit(Amps.of(60)) .withStatorCurrentLimit(Amps.of(60))
.withStatorCurrentLimitEnable(true) .withStatorCurrentLimitEnable(true)
); );
private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
// Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
private static final Pigeon2Configuration pigeonConfigs = null; private static final Pigeon2Configuration pigeonConfigs = null;
// CAN bus that the devices are located on; // CAN bus that the devices are located on;
// All swerve devices must share the same CAN bus // All swerve devices must share the same CAN bus
public static final CANBus kCANBus = new CANBus("swerve", "./logs/example.hoot"); public static final CANBus kCANBus = new CANBus("swerve", "./logs/example.hoot");
// Theoretical free speed (m/s) at 12 V applied output; // Theoretical free speed (m/s) at 12 V applied output;
// This needs to be tuned to your individual robot // This needs to be tuned to your individual robot
public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(9.82); public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(9.82);
// Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
// This may need to be tuned to your individual robot // This may need to be tuned to your individual robot
private static final double kCoupleRatio = 3.5714285714285716; private static final double kCoupleRatio = 3.5714285714285716;
private static final double kDriveGearRatio = 6.122448979591837; private static final double kDriveGearRatio = 6.122448979591837;
private static final double kSteerGearRatio = 21.428571428571427; private static final double kSteerGearRatio = 21.428571428571427;
private static final Distance kWheelRadius = Inches.of(3.895); // private static final Distance kWheelRadius = Inches.of(3.895);
private static final Distance kWheelRadius = Inches.of(2);
private static final boolean kInvertLeftSide = false;
private static final boolean kInvertRightSide = true; private static final boolean kInvertLeftSide = false;
private static final boolean kInvertRightSide = true;
private static final int kPigeonId = 16;
private static final int kPigeonId = 16;
// These are only used for simulation
private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); // These are only used for simulation
private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01);
// Simulated voltage necessary to overcome friction private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01);
private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); // Simulated voltage necessary to overcome friction
private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
.withCANBusName(kCANBus.getName()) public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
.withPigeon2Id(kPigeonId) .withCANBusName(kCANBus.getName())
.withPigeon2Configs(pigeonConfigs); .withPigeon2Id(kPigeonId)
.withPigeon2Configs(pigeonConfigs);
private static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() private static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
.withDriveMotorGearRatio(kDriveGearRatio) new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>()
.withSteerMotorGearRatio(kSteerGearRatio) .withDriveMotorGearRatio(kDriveGearRatio)
.withCouplingGearRatio(kCoupleRatio) .withSteerMotorGearRatio(kSteerGearRatio)
.withWheelRadius(kWheelRadius) .withCouplingGearRatio(kCoupleRatio)
.withSteerMotorGains(steerGains) .withWheelRadius(kWheelRadius)
.withDriveMotorGains(driveGains) .withSteerMotorGains(steerGains)
.withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) .withDriveMotorGains(driveGains)
.withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
.withSlipCurrent(kSlipCurrent) .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
.withSpeedAt12Volts(kSpeedAt12Volts) .withSlipCurrent(kSlipCurrent)
.withDriveMotorType(kDriveMotorType) .withSpeedAt12Volts(kSpeedAt12Volts)
.withSteerMotorType(kSteerMotorType) .withDriveMotorType(kDriveMotorType)
.withFeedbackSource(kSteerFeedbackType) .withSteerMotorType(kSteerMotorType)
.withDriveMotorInitialConfigs(driveInitialConfigs) .withFeedbackSource(kSteerFeedbackType)
.withSteerMotorInitialConfigs(steerInitialConfigs) .withDriveMotorInitialConfigs(driveInitialConfigs)
.withEncoderInitialConfigs(encoderInitialConfigs) .withSteerMotorInitialConfigs(steerInitialConfigs)
.withSteerInertia(kSteerInertia) .withEncoderInitialConfigs(encoderInitialConfigs)
.withDriveInertia(kDriveInertia) .withSteerInertia(kSteerInertia)
.withSteerFrictionVoltage(kSteerFrictionVoltage) .withDriveInertia(kDriveInertia)
.withDriveFrictionVoltage(kDriveFrictionVoltage); .withSteerFrictionVoltage(kSteerFrictionVoltage)
.withDriveFrictionVoltage(kDriveFrictionVoltage);
// Front Left
private static final int kFrontLeftDriveMotorId = 6; // Front Left
private static final int kFrontLeftSteerMotorId = 9; private static final int kFrontLeftDriveMotorId = 6;
private static final int kFrontLeftEncoderId = 24; private static final int kFrontLeftSteerMotorId = 9;
private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.36767578125); private static final int kFrontLeftEncoderId = 24;
private static final boolean kFrontLeftSteerMotorInverted = true; private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.36767578125);
private static final boolean kFrontLeftEncoderInverted = false; private static final boolean kFrontLeftSteerMotorInverted = true;
private static final boolean kFrontLeftEncoderInverted = false;
private static final Distance kFrontLeftXPos = Inches.of(11.375);
private static final Distance kFrontLeftYPos = Inches.of(10.625); private static final Distance kFrontLeftXPos = Inches.of(11.375);
private static final Distance kFrontLeftYPos = Inches.of(10.625);
// Front Right
private static final int kFrontRightDriveMotorId = 7; // Front Right
private static final int kFrontRightSteerMotorId = 5; private static final int kFrontRightDriveMotorId = 7;
private static final int kFrontRightEncoderId = 22; private static final int kFrontRightSteerMotorId = 5;
private static final Angle kFrontRightEncoderOffset = Rotations.of(0.23486328125); private static final int kFrontRightEncoderId = 22;
private static final boolean kFrontRightSteerMotorInverted = true; private static final Angle kFrontRightEncoderOffset = Rotations.of(0.23486328125);
private static final boolean kFrontRightEncoderInverted = false; private static final boolean kFrontRightSteerMotorInverted = true;
private static final boolean kFrontRightEncoderInverted = false;
private static final Distance kFrontRightXPos = Inches.of(11.375);
private static final Distance kFrontRightYPos = Inches.of(-10.625); private static final Distance kFrontRightXPos = Inches.of(11.375);
private static final Distance kFrontRightYPos = Inches.of(-10.625);
// Back Left
private static final int kBackLeftDriveMotorId = 10; // Back Left
private static final int kBackLeftSteerMotorId = 11; private static final int kBackLeftDriveMotorId = 10;
private static final int kBackLeftEncoderId = 21; private static final int kBackLeftSteerMotorId = 11;
private static final Angle kBackLeftEncoderOffset = Rotations.of(0.202392578125); private static final int kBackLeftEncoderId = 21;
private static final boolean kBackLeftSteerMotorInverted = true; private static final Angle kBackLeftEncoderOffset = Rotations.of(0.202392578125);
private static final boolean kBackLeftEncoderInverted = false; private static final boolean kBackLeftSteerMotorInverted = true;
private static final boolean kBackLeftEncoderInverted = false;
private static final Distance kBackLeftXPos = Inches.of(-11.375);
private static final Distance kBackLeftYPos = Inches.of(10.625); private static final Distance kBackLeftXPos = Inches.of(-11.375);
private static final Distance kBackLeftYPos = Inches.of(10.625);
// Back Right
private static final int kBackRightDriveMotorId = 14; // Back Right
private static final int kBackRightSteerMotorId = 13; private static final int kBackRightDriveMotorId = 14;
private static final int kBackRightEncoderId = 23; private static final int kBackRightSteerMotorId = 13;
private static final Angle kBackRightEncoderOffset = Rotations.of(-0.33203125); private static final int kBackRightEncoderId = 23;
private static final boolean kBackRightSteerMotorInverted = true; private static final Angle kBackRightEncoderOffset = Rotations.of(-0.33203125);
private static final boolean kBackRightEncoderInverted = false; private static final boolean kBackRightSteerMotorInverted = true;
private static final boolean kBackRightEncoderInverted = false;
private static final Distance kBackRightXPos = Inches.of(-11.375);
private static final Distance kBackRightYPos = Inches.of(-10.625); private static final Distance kBackRightXPos = Inches.of(-11.375);
private static final Distance kBackRightYPos = Inches.of(-10.625);
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontLeft =
ConstantCreator.createModuleConstants( public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontLeft =
kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, ConstantCreator.createModuleConstants(
kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset,
); kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontRight = );
ConstantCreator.createModuleConstants( public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontRight =
kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, ConstantCreator.createModuleConstants(
kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset,
); kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackLeft = );
ConstantCreator.createModuleConstants( public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackLeft =
kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, ConstantCreator.createModuleConstants(
kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset,
); kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackRight = );
ConstantCreator.createModuleConstants( public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackRight =
kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, ConstantCreator.createModuleConstants(
kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset,
); kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted
);
/**
* Creates a CommandSwerveDrivetrain instance. /**
* This should only be called once in your robot program,. * Creates a CommandSwerveDrivetrain instance.
*/ * This should only be called once in your robot program,.
public static CommandSwerveDrivetrain createDrivetrain() { */
return new CommandSwerveDrivetrain( public static CommandSwerveDrivetrain createDrivetrain() {
DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight return new CommandSwerveDrivetrain(
); DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight
} );
}
/**
* Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. /**
*/ * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types.
public static class TunerSwerveDrivetrain extends SwerveDrivetrain<TalonFX, TalonFX, CANcoder> { */
/** public static class TunerSwerveDrivetrain extends SwerveDrivetrain<TalonFX, TalonFX, CANcoder> {
* Constructs a CTRE SwerveDrivetrain using the specified constants. /**
* <p> * Constructs a CTRE SwerveDrivetrain using the specified constants.
* This constructs the underlying hardware devices, so users should not construct * <p>
* the devices themselves. If they need the devices, they can access them through * This constructs the underlying hardware devices, so users should not construct
* getters in the classes. * the devices themselves. If they need the devices, they can access them through
* * getters in the classes.
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive *
* @param modules Constants for each specific module * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
*/ * @param modules Constants for each specific module
public TunerSwerveDrivetrain( */
SwerveDrivetrainConstants drivetrainConstants, public TunerSwerveDrivetrain(
SwerveModuleConstants<?, ?, ?>... modules SwerveDrivetrainConstants drivetrainConstants,
) { SwerveModuleConstants<?, ?, ?>... modules
super( ) {
TalonFX::new, TalonFX::new, CANcoder::new, super(
drivetrainConstants, modules TalonFX::new, TalonFX::new, CANcoder::new,
); drivetrainConstants, modules
} );
}
/**
* Constructs a CTRE SwerveDrivetrain using the specified constants. /**
* <p> * Constructs a CTRE SwerveDrivetrain using the specified constants.
* This constructs the underlying hardware devices, so users should not construct * <p>
* the devices themselves. If they need the devices, they can access them through * This constructs the underlying hardware devices, so users should not construct
* getters in the classes. * the devices themselves. If they need the devices, they can access them through
* * getters in the classes.
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive *
* @param odometryUpdateFrequency The frequency to run the odometry loop. If * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
* unspecified or set to 0 Hz, this is 250 Hz on * @param odometryUpdateFrequency The frequency to run the odometry loop. If
* CAN FD, and 100 Hz on CAN 2.0. * unspecified or set to 0 Hz, this is 250 Hz on
* @param modules Constants for each specific module * CAN FD, and 100 Hz on CAN 2.0.
*/ * @param modules Constants for each specific module
public TunerSwerveDrivetrain( */
SwerveDrivetrainConstants drivetrainConstants, public TunerSwerveDrivetrain(
double odometryUpdateFrequency, SwerveDrivetrainConstants drivetrainConstants,
SwerveModuleConstants<?, ?, ?>... modules double odometryUpdateFrequency,
) { SwerveModuleConstants<?, ?, ?>... modules
super( ) {
TalonFX::new, TalonFX::new, CANcoder::new, super(
drivetrainConstants, odometryUpdateFrequency, modules TalonFX::new, TalonFX::new, CANcoder::new,
); drivetrainConstants, odometryUpdateFrequency, modules
} );
}
/**
* Constructs a CTRE SwerveDrivetrain using the specified constants. /**
* <p> * Constructs a CTRE SwerveDrivetrain using the specified constants.
* This constructs the underlying hardware devices, so users should not construct * <p>
* the devices themselves. If they need the devices, they can access them through * This constructs the underlying hardware devices, so users should not construct
* getters in the classes. * the devices themselves. If they need the devices, they can access them through
* * getters in the classes.
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive *
* @param odometryUpdateFrequency The frequency to run the odometry loop. If * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
* unspecified or set to 0 Hz, this is 250 Hz on * @param odometryUpdateFrequency The frequency to run the odometry loop. If
* CAN FD, and 100 Hz on CAN 2.0. * unspecified or set to 0 Hz, this is 250 Hz on
* @param odometryStandardDeviation The standard deviation for odometry calculation * CAN FD, and 100 Hz on CAN 2.0.
* in the form [x, y, theta]ᵀ, with units in meters * @param odometryStandardDeviation The standard deviation for odometry calculation
* and radians * in the form [x, y, theta]ᵀ, with units in meters
* @param visionStandardDeviation The standard deviation for vision calculation * and radians
* in the form [x, y, theta]ᵀ, with units in meters * @param visionStandardDeviation The standard deviation for vision calculation
* and radians * in the form [x, y, theta]ᵀ, with units in meters
* @param modules Constants for each specific module * and radians
*/ * @param modules Constants for each specific module
public TunerSwerveDrivetrain( */
SwerveDrivetrainConstants drivetrainConstants, public TunerSwerveDrivetrain(
double odometryUpdateFrequency, SwerveDrivetrainConstants drivetrainConstants,
Matrix<N3, N1> odometryStandardDeviation, double odometryUpdateFrequency,
Matrix<N3, N1> visionStandardDeviation, Matrix<N3, N1> odometryStandardDeviation,
SwerveModuleConstants<?, ?, ?>... modules Matrix<N3, N1> visionStandardDeviation,
) { SwerveModuleConstants<?, ?, ?>... modules
super( ) {
TalonFX::new, TalonFX::new, CANcoder::new, super(
drivetrainConstants, odometryUpdateFrequency, TalonFX::new, TalonFX::new, CANcoder::new,
odometryStandardDeviation, visionStandardDeviation, modules drivetrainConstants, odometryUpdateFrequency,
); odometryStandardDeviation, visionStandardDeviation, modules
} );
} }
} }
}

2
src/main/java/frc/robot/subsystems/Grimpeur.java
View File

@@ -36,7 +36,7 @@ public class Grimpeur extends SubsystemBase {
return limit.get(); return limit.get();
} }
public double PositionFinal(){ public double PositionFinal(){
return EncodeurGrimpeur.getDouble(105); return EncodeurGrimpeur.getDouble(101);
} }
/** Creates a new Grimpeur. */ /** Creates a new Grimpeur. */
public Grimpeur() { public Grimpeur() {