diff --git a/src/main/deploy/pathplanner/paths/TirerSimple.path b/src/main/deploy/pathplanner/paths/TirerSimple.path index 60f5642..f826a15 100644 --- a/src/main/deploy/pathplanner/paths/TirerSimple.path +++ b/src/main/deploy/pathplanner/paths/TirerSimple.path @@ -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, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index f6fcc16..c80284c 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -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, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f40b094..8bcbc56 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -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)); diff --git a/src/main/java/frc/robot/commands/Lancer.java b/src/main/java/frc/robot/commands/Lancer.java index ff3ca28..f090317 100644 --- a/src/main/java/frc/robot/commands/Lancer.java +++ b/src/main/java/frc/robot/commands/Lancer.java @@ -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 = 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); diff --git a/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java index 7aa8bc3..a617c26 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java @@ -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 = 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); diff --git a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java index 132980c..516fec2 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java @@ -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; diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 430f31b..fe935e5 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -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 ConstantCreator = - new SwerveModuleConstantsFactory() - .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 FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, - kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted - ); - public static final SwerveModuleConstants FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, - kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted - ); - public static final SwerveModuleConstants BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, - kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted - ); - public static final SwerveModuleConstants 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 { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * 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. - *

- * 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. - *

- * 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 odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, - odometryStandardDeviation, visionStandardDeviation, modules - ); - } - } -} \ No newline at end of file +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 ConstantCreator = + new SwerveModuleConstantsFactory() + .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 FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, + kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted + ); + public static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted + ); + public static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted + ); + public static final SwerveModuleConstants 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 { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * 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. + *

+ * 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. + *

+ * 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 odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules + ); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/Grimpeur.java b/src/main/java/frc/robot/subsystems/Grimpeur.java index da425cf..0cfbad1 100644 --- a/src/main/java/frc/robot/subsystems/Grimpeur.java +++ b/src/main/java/frc/robot/subsystems/Grimpeur.java @@ -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() {