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

debug samedi

Browse Source
This commit is contained in:
samuel desharnais
2026-03-28 13:20:52 -04:00
parent 79568a58b9
commit ba57959f4d
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": [
{
"anchor": {
"x": 3.5473894436519258,
"x": 3.602,
"y": 4.015592011412268
},
"prevControl": null,
"nextControl": {
"x": 2.494897544356074,
"x": 2.549508100704148,
"y": 4.011425161481168
},
"isLocked": false,
@@ -16,12 +16,12 @@
},
{
"anchor": {
"x": 1.7359771754636228,
"x": 1.643730569948188,
"y": 4.015592011412268
},
"prevControl": {
"x": 2.627549770741877,
"y": 4.02839075681212
"x": 2.5679620034542325,
"y": 4.011502590673576
},
"nextControl": null,
"isLocked": false,

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

@@ -16,12 +16,12 @@
"robotMass": 51.673,
"robotMOI": 6.883,
"robotTrackwidth": 0.546,
"driveWheelRadius": 0.099,
"driveWheelRadius": 0.051,
"driveGearing": 6.2,
"maxDriveSpeed": 9.82,
"driveMotorType": "krakenX60",
"driveCurrentLimit": 120.0,
"wheelCOF": 1.1,
"wheelCOF": 1.2,
"flModuleX": 0.288925,
"flModuleY": 0.269875,
"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.GrimperReservoir;
import frc.robot.commands.ModeAuto.LancerAuto;
import frc.robot.commands.ModeAuto.LimelighterAuto;
import frc.robot.commands.ModeAuto.RetourMilieuDroite;
import frc.robot.commands.ModeAuto.RetourMilieuGauche;
import frc.robot.commands.ModeAuto.TournerVersMur;
@@ -77,7 +78,7 @@ public class RobotContainer {
NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur, limeLight3G));
NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite(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("Aspirer", new AspirerAuto(balayeuse));
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;
import java.util.Optional;
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.wpilibj2.command.Command;
import frc.robot.subsystems.Lanceur;
@@ -16,6 +20,11 @@ public class Lancer extends Command {
private PIDController pidController;
private Limelight3G limeLight3G;
private Timer timer;
double vitesse = 0.5;
double botx = 0;
double boty = 0;
Optional<Alliance> alliance = DriverStation.getAlliance();
/** Creates a new Lancer. */
public Lancer(Lanceur lanceur, Limelight3G limeLight3G) {
this.lanceur = lanceur;
@@ -28,6 +37,7 @@ public class Lancer extends Command {
// Called when the command is initially scheduled.
@Override
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);
timer.reset();
}
@@ -35,17 +45,19 @@ public class Lancer extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double botx = 0;
double boty = 0;
double[] BotPose = new double[6];
if(limeLight3G.getV()){
BotPose = limeLight3G.getBotPoseBlue();
if(alliance.get() == Alliance.Blue){
BotPose = limeLight3G.getBotPoseBlue();
}
else{
BotPose = limeLight3G.getBotPoseRed();
}
botx = BotPose[0];
boty = BotPose[1];
}
double vitesse = 0.5;
}
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);
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;
import java.util.Optional;
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.wpilibj2.command.Command;
import frc.robot.subsystems.Lanceur;
@@ -18,6 +22,7 @@ public class LancerAuto extends Command {
Limelight3G limelight3g;
double botx = 0;
double boty = 0;
Optional<Alliance> alliance = DriverStation.getAlliance();
/** Creates a new LancerAuto. */
public LancerAuto(Lanceur lanceur, Limelight3G limelight3g) {
this.lanceur = lanceur;
@@ -38,13 +43,19 @@ public class LancerAuto extends Command {
public void execute() {
double[] BotPose = new double[6];
if(limelight3g.getV()){
BotPose = limelight3g.getBotPoseBlue();
if(alliance.get() == Alliance.Blue){
BotPose = limelight3g.getBotPoseBlue();
}
else{
BotPose = limelight3g.getBotPoseRed();
}
botx = BotPose[0];
boty = BotPose[1];
}
double vitesse = 0.5;
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);
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.subsystems.CommandSwerveDrivetrain;
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;

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

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