* Driver controls for the PS5 controller
*/
public class PS5ControllerDriverConfig extends BaseDriverConfig {
- private final PS5Controller controller = new PS5Controller(Constants.DRIVER_JOY);
- private final BooleanSupplier slowModeSupplier = () -> false;
- private boolean intakeBoolean = true;
- private Command autoShoot = null;
- private Shooter shooter;
- private Turret turret;
- private Hood hood;
- private Intake intake;
- private Spindexer spindexer;
-
- public PS5ControllerDriverConfig(
+ private final PS5Controller controller = new PS5Controller(Constants.DRIVER_JOY);
+ private final BooleanSupplier slowModeSupplier = () -> false;
+ private boolean intakeBoolean = true;
+ private Command autoShoot = null;
+ private Shooter shooter;
+ private Turret turret;
+ private Hood hood;
+ private Intake intake;
+ private Spindexer spindexer;
+
+ public PS5ControllerDriverConfig(
Drivetrain drive,
Shooter shooter,
Turret turret,
Hood hood,
Intake intake,
- Spindexer spindexer
- ) {
+ Spindexer spindexer) {
super(drive);
this.shooter = shooter;
this.turret = turret;
this.spindexer = spindexer;
}
- public void configureControls() {
- // Reset the yaw. Mainly useful for testing/driver practice
- controller.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
- new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
-
- // Cancel commands
- controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> {
- getDrivetrain().setIsAlign(false);
- getDrivetrain().setDesiredPose(() -> null);
- CommandScheduler.getInstance().cancelAll();
- }));
-
- // Reverse motors
- if (intake != null && spindexer != null) {
- controller.get(PS5Button.LB).whileTrue(new ReverseMotors(intake));
- }
+ public void configureControls() {
+ // Reset the yaw. Mainly useful for testing/driver practice
+ controller.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
+
+ // Cancel commands
+ controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> {
+ getDrivetrain().setIsAlign(false);
+ getDrivetrain().setDesiredPose(() -> null);
+ CommandScheduler.getInstance().cancelAll();
+ }));
+
+ // Reverse motors
+ if (intake != null && spindexer != null) {
+ controller.get(PS5Button.LB).whileTrue(new ReverseMotors(intake));
+ }
- // Intake
- if (intake != null) {
- // Toggle intake
- controller.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
- if (intakeBoolean) {
- intake.extend();
- intake.spinStart();
- intakeBoolean = false;
- } else {
- intake.intermediateExtend();
- intake.spinStop();
- intakeBoolean = true;
+ // Intake
+ if (intake != null) {
+ // Toggle intake
+ controller.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
+ if (intakeBoolean) {
+ intake.extend();
+ intake.spinStart();
+ intakeBoolean = false;
+ } else {
+ intake.intermediateExtend();
+ intake.spinStop();
+ intakeBoolean = true;
+ }
+ }, intake));
+
+ // Retract if hold for 2 seconds
+ controller.get(PS5Button.RIGHT_TRIGGER).debounce(2.0).onTrue(new InstantCommand(() -> {
+ intake.retract();
+ intakeBoolean = true;
+ intake.spinStop();
+ }, intake));
+
+ // Make the intake go in and out while shooting
+ controller.get(DPad.UP).whileTrue(new IntakeMovementCommand(intake)
+ .alongWith(new InstantCommand(()-> intakeBoolean = true)));
+
+ // Calibration: you can now calibrate easily using this button
+ if (hood != null && intake != null) {
+ controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
+ intake.calibrate();
+ hood.calibrate();
+ }, intake, hood)).onFalse(new InstantCommand(() -> {
+ intake.stopCalibrating();
+ hood.stopCalibrating();
+ }, intake, hood));
+ }
+
+ // Stop intake roller
+ controller.get(DPad.DOWN).onTrue(new InstantCommand(()->{
+ if(intakeBoolean){
+ intake.spinStart();
+ intakeBoolean = false;
+ } else{
+ intake.spinStop();
+ intakeBoolean = true;
+ }
+ }));
+ }
+
+ // Spindexer
+ if (spindexer != null && turret != null && hood != null && intake != null) {
+
+ // Toggle spindexer
+ controller.get(PS5Button.LEFT_TRIGGER).toggleOnTrue(
+ new RunSpindexer(spindexer, turret, hood, intake)
+ );
+ }
+
+ // Auto shoot
+ if (turret != null && hood != null && shooter != null && spindexer != null) {
+ autoShoot = new Superstructure(turret, getDrivetrain(), hood, shooter, spindexer);
+ controller.get(PS5Button.SQUARE).toggleOnTrue(autoShoot);
+ }
+
+
+ // Hood
+ if (hood != null) {
+ // Set the hood down -- for safety measures under trench
+ controller.get(DPad.LEFT).onTrue(new InstantCommand(()->{
+ hood.forceHoodDown(true);
+ }, hood)).onFalse(new InstantCommand(()->{
+ hood.forceHoodDown(false);
+ }));
+ }
+
+ // shoot focus mode: reduces drive current
+ if (spindexer != null) {
+ controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> shootFocus(true)))
+ .onFalse(new InstantCommand(()-> shootFocus(false)));
}
- }, intake));
-
- // Retract if hold for 2 seconds
- controller.get(PS5Button.RIGHT_TRIGGER).debounce(2.0).onTrue(new InstantCommand(() -> {
- intake.retract();
- intakeBoolean = true;
- intake.spinStop();
- }, intake));
-
- // Make the intake go in and out while shooting
- controller.get(DPad.UP).whileTrue(new IntakeMovementCommand(intake)
- .alongWith(new InstantCommand(() -> intakeBoolean = true)));
-
- // Calibration: you can now calibrate easily using this button
- if (hood != null && intake != null) {
- controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
- intake.calibrate();
- hood.calibrate();
- }, intake, hood)).onFalse(new InstantCommand(() -> {
- intake.stopCalibrating();
- hood.stopCalibrating();
- }, intake, hood));
- }
-
- // Stop intake roller
- controller.get(DPad.DOWN).onTrue(new InstantCommand(() -> {
- if (intakeBoolean) {
- intake.spinStart();
- intakeBoolean = false;
+ }
+
+ private void shootFocus(boolean turnOn) {
+ if (turnOn) {
+ System.out.println("Shooting is now Focused");
+ spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit);
+ drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT,
+ DriveConstants.DRIVE_PEAK_CURRENT_LIMIT * 0.25);
} else {
- intake.spinStop();
- intakeBoolean = true;
+ System.out.println("Shooting back to normal (From focus)");
+ spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit);
+ drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT,
+ DriveConstants.DRIVE_PEAK_CURRENT_LIMIT);
}
- }));
}
- // Spindexer
- if (spindexer != null && turret != null && hood != null && intake != null) {
+ @Override
+ public double getRawSideTranslation() {
+ return controller.get(PS5Axis.LEFT_X);
+ }
+
+ @Override
+ public double getRawForwardTranslation() {
+ return controller.get(PS5Axis.LEFT_Y);
+ }
- // Toggle spindexer
- controller.get(PS5Button.LEFT_TRIGGER).toggleOnTrue(
- new RunSpindexer(spindexer, turret, hood, intake));
+ @Override
+ public double getRawRotation() {
+ return controller.get(PS5Axis.RIGHT_X);
}
- // Auto shoot
- if (turret != null && hood != null && shooter != null && spindexer != null) {
- autoShoot = new Superstructure(turret, getDrivetrain(), hood, shooter, spindexer);
- controller.get(PS5Button.SQUARE).toggleOnTrue(autoShoot);
+ @Override
+ public double getRawHeadingAngle() {
+ return Math.atan2(controller.get(PS5Axis.RIGHT_X), -controller.get(PS5Axis.RIGHT_Y)) - Math.PI / 2;
}
- // Hood
- if (hood != null) {
- // Set the hood down -- for safety measures under trench
- controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> {
- hood.forceHoodDown(true);
- }, hood)).onFalse(new InstantCommand(() -> {
- hood.forceHoodDown(false);
- }));
+ @Override
+ public double getRawHeadingMagnitude() {
+ return Math.hypot(controller.get(PS5Axis.RIGHT_X), controller.get(PS5Axis.RIGHT_Y));
}
- // shoot focus mode: reduces drive current
- if (spindexer != null) {
- controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> shootFocus(true)))
- .onFalse(new InstantCommand(() -> shootFocus(false)));
+ @Override
+ public boolean getIsSlowMode() {
+ return slowModeSupplier.getAsBoolean();
}
- }
-
- private void shootFocus(boolean turnOn) {
- if (turnOn) {
- System.out.println("Shooting is now Focused");
- spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit);
- drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT,
- DriveConstants.DRIVE_PEAK_CURRENT_LIMIT * 0.25);
- } else {
- System.out.println("Shooting back to normal (From focus)");
- spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit);
- drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT, DriveConstants.DRIVE_PEAK_CURRENT_LIMIT);
+
+ @Override
+ public boolean getIsAlign() {
+ return false;
}
- }
-
- @Override
- public double getRawSideTranslation() {
- return controller.get(PS5Axis.LEFT_X);
- }
-
- @Override
- public double getRawForwardTranslation() {
- return controller.get(PS5Axis.LEFT_Y);
- }
-
- @Override
- public double getRawRotation() {
- return controller.get(PS5Axis.RIGHT_X);
- }
-
- @Override
- public double getRawHeadingAngle() {
- return Math.atan2(controller.get(PS5Axis.RIGHT_X), -controller.get(PS5Axis.RIGHT_Y)) - Math.PI / 2;
- }
-
- @Override
- public double getRawHeadingMagnitude() {
- return Math.hypot(controller.get(PS5Axis.RIGHT_X), controller.get(PS5Axis.RIGHT_Y));
- }
-
- @Override
- public boolean getIsSlowMode() {
- return slowModeSupplier.getAsBoolean();
- }
-
- @Override
- public boolean getIsAlign() {
- return false;
- }
}
*/
public class PS5XboxModeDriverConfig extends BaseDriverConfig {
- private final GameController controller = new GameController(Constants.DRIVER_JOY);
- private final BooleanSupplier slowModeSupplier = () -> false;
- private boolean intakeBoolean = true;
- private Command autoShoot = null;
- private Command reverseMotors = null;
- private Shooter shooter;
- private Turret turret;
- private Hood hood;
- private Intake intake;
- private Spindexer spindexer;
-
- // PS5 button aliases
- private final Button CROSS = Button.A;
- private final Button CIRCLE = Button.B;
- private final Button SQUARE = Button.X;
- private final Button TRIANGLE = Button.Y;
- private final Button LB = Button.LB;
- private final Button RB = Button.RB;
- private final Button CREATE = Button.BACK;
- private final Button OPTIONS = Button.START;
- private final Button LEFT_JOY = Button.LEFT_JOY;
- private final Button RIGHT_JOY = Button.RIGHT_JOY;
-
- // PS5 trigger buttons
- private final BooleanSupplier LEFT_TRIGGER_BUTTON = controller.LEFT_TRIGGER_BUTTON;
- private final BooleanSupplier RIGHT_TRIGGER_BUTTON = controller.RIGHT_TRIGGER_BUTTON;
-
- // PS5 axis aliases
- private final Axis LEFT_X = Axis.LEFT_X;
- private final Axis LEFT_Y = Axis.LEFT_Y;
- private final Axis RIGHT_X = Axis.RIGHT_X;
- private final Axis RIGHT_Y = Axis.RIGHT_Y;
- private final Axis LEFT_TRIGGER = Axis.LEFT_TRIGGER;
- private final Axis RIGHT_TRIGGER = Axis.RIGHT_TRIGGER;
-
- public PS5XboxModeDriverConfig(
- Drivetrain drive,
- Shooter shooter,
- Turret turret,
- Hood hood,
- Intake intake,
- Spindexer spindexer) {
- super(drive);
- this.shooter = shooter;
- this.turret = turret;
- this.hood = hood;
- this.intake = intake;
- this.spindexer = spindexer;
- }
-
- public void configureControls() {
- // Reset the yaw. Mainly useful for testing/driver practice
- controller.get(CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
- new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
-
- // Cancel commands
- controller.get(RB).onTrue(new InstantCommand(() -> {
- getDrivetrain().setIsAlign(false);
- getDrivetrain().setDesiredPose(() -> null);
- CommandScheduler.getInstance().cancelAll();
- }));
-
- // Align wheels
- controller.get(DPad.RIGHT).onTrue(new FunctionalCommand(
- () -> getDrivetrain().setStateDeadband(false),
- getDrivetrain()::alignWheels,
- interrupted -> getDrivetrain().setStateDeadband(true),
- () -> false, getDrivetrain()).withTimeout(2));
-
- // Trench align
- controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> {
- getDrivetrain().setTrenchAssist(true);
- getDrivetrain().setTrenchAlign(true);
- }))
- .onFalse(new InstantCommand(() -> {
- getDrivetrain().setTrenchAssist(false);
- getDrivetrain().setTrenchAlign(false);
+ private final GameController controller = new GameController(Constants.DRIVER_JOY);
+ private final BooleanSupplier slowModeSupplier = () -> false;
+ private boolean intakeBoolean = true;
+ private Command autoShoot = null;
+ private Command reverseMotors = null;
+ private Shooter shooter;
+ private Turret turret;
+ private Hood hood;
+ private Intake intake;
+ private Spindexer spindexer;
+
+ // PS5 button aliases
+ private final Button CROSS = Button.A;
+ private final Button CIRCLE = Button.B;
+ private final Button SQUARE = Button.X;
+ private final Button TRIANGLE = Button.Y;
+ private final Button LB = Button.LB;
+ private final Button RB = Button.RB;
+ private final Button CREATE = Button.BACK;
+ private final Button OPTIONS = Button.START;
+ private final Button LEFT_JOY = Button.LEFT_JOY;
+ private final Button RIGHT_JOY = Button.RIGHT_JOY;
+
+ // PS5 trigger buttons
+ private final BooleanSupplier LEFT_TRIGGER_BUTTON = controller.LEFT_TRIGGER_BUTTON;
+ private final BooleanSupplier RIGHT_TRIGGER_BUTTON = controller.RIGHT_TRIGGER_BUTTON;
+
+ // PS5 axis aliases
+ private final Axis LEFT_X = Axis.LEFT_X;
+ private final Axis LEFT_Y = Axis.LEFT_Y;
+ private final Axis RIGHT_X = Axis.RIGHT_X;
+ private final Axis RIGHT_Y = Axis.RIGHT_Y;
+ private final Axis LEFT_TRIGGER = Axis.LEFT_TRIGGER;
+ private final Axis RIGHT_TRIGGER = Axis.RIGHT_TRIGGER;
+
+ public PS5XboxModeDriverConfig(
+ Drivetrain drive,
+ Shooter shooter,
+ Turret turret,
+ Hood hood,
+ Intake intake,
+ Spindexer spindexer) {
+ super(drive);
+ this.shooter = shooter;
+ this.turret = turret;
+ this.hood = hood;
+ this.intake = intake;
+ this.spindexer = spindexer;
+ }
+
+ public void configureControls() {
+ // Reset the yaw. Mainly useful for testing/driver practice
+ controller.get(CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
+
+ // Cancel commands
+ controller.get(RB).onTrue(new InstantCommand(() -> {
+ getDrivetrain().setIsAlign(false);
+ getDrivetrain().setDesiredPose(() -> null);
+ CommandScheduler.getInstance().cancelAll();
}));
- // Reverse motors
- if (intake != null && spindexer != null && shooter != null) {
- controller.get(CIRCLE).onTrue(new InstantCommand(() -> {
- reverseMotors = new ReverseMotors(intake);
- CommandScheduler.getInstance().schedule(reverseMotors);
- })).onFalse(new InstantCommand(() -> {
- if (reverseMotors != null) {
- reverseMotors.cancel();
+ // Align wheels
+ controller.get(DPad.RIGHT).onTrue(new FunctionalCommand(
+ () -> getDrivetrain().setStateDeadband(false),
+ getDrivetrain()::alignWheels,
+ interrupted -> getDrivetrain().setStateDeadband(true),
+ () -> false, getDrivetrain()).withTimeout(2));
+
+ // Trench align
+ controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> {
+ getDrivetrain().setTrenchAssist(true);
+ getDrivetrain().setTrenchAlign(true);
+ }))
+ .onFalse(new InstantCommand(() -> {
+ getDrivetrain().setTrenchAssist(false);
+ getDrivetrain().setTrenchAlign(false);
+ }));
+
+ // Reverse motors
+ if (intake != null && spindexer != null && shooter != null) {
+ controller.get(CIRCLE).onTrue(new InstantCommand(() -> {
+ reverseMotors = new ReverseMotors(intake);
+ CommandScheduler.getInstance().schedule(reverseMotors);
+ })).onFalse(new InstantCommand(() -> {
+ if (reverseMotors != null) {
+ reverseMotors.cancel();
+ }
+ }));
+ }
+
+ // Intake
+ if (intake != null) {
+ // Toggle intake
+ controller.get(RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> {
+ if (intakeBoolean) {
+ intake.extend();
+ intake.spinStart();
+ intakeBoolean = false;
+ } else {
+ intake.intermediateExtend();
+ intake.spinStop();
+ intakeBoolean = true;
+ }
+ }));
+
+ // Retract if hold for 3 seconds
+ controller.get(RIGHT_TRIGGER_BUTTON).debounce(3.0).onTrue(new InstantCommand(() -> {
+ intake.retract();
+ intakeBoolean = true;
+ }));
+
+ // Calibration
+ controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> {
+ intake.calibrate();
+ })).onFalse(new InstantCommand(() -> {
+ intake.stopCalibrating();
+ }));
}
- }));
- }
- // Intake
- if (intake != null) {
- // Toggle intake
- controller.get(RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> {
- if (intakeBoolean) {
- intake.extend();
- intake.spinStart();
- intakeBoolean = false;
- } else {
- intake.intermediateExtend();
- intake.spinStop();
- intakeBoolean = true;
+ // Spindexer
+ if (spindexer != null) {
+ // Will only run if we are not calling default shoot command
+ controller.get(LEFT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> spindexer.maxSpindexer()))
+ .onFalse(new InstantCommand(() -> spindexer.stopSpindexer()));
}
- }));
-
- // Retract if hold for 3 seconds
- controller.get(RIGHT_TRIGGER_BUTTON).debounce(3.0).onTrue(new InstantCommand(() -> {
- intake.retract();
- intakeBoolean = true;
- }));
-
- // Calibration
- controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> {
- intake.calibrate();
- })).onFalse(new InstantCommand(() -> {
- intake.stopCalibrating();
- }));
+
+ // Auto shoot
+ if (turret != null && hood != null && shooter != null) {
+ controller.get(SQUARE).onTrue(
+ new InstantCommand(() -> {
+ if (autoShoot != null && autoShoot.isScheduled()) {
+ autoShoot.cancel();
+ } else {
+ autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer);
+ CommandScheduler.getInstance().schedule(autoShoot);
+ }
+ }));
+ }
+
+ // Hood
+ if (hood != null) {
+ controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> {
+ hood.calibrate();
+ })).onFalse(new InstantCommand(() -> {
+ hood.stopCalibrating();
+ }));
+ }
+
+ // Rumble test
+ controller.get(RIGHT_JOY).onTrue(new SequentialCommandGroup(
+ new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_ON)),
+ new WaitCommand(0.5),
+ new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF))));
}
- // Spindexer
- if (spindexer != null) {
- // Will only run if we are not calling default shoot command
- controller.get(LEFT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> spindexer.maxSpindexer()))
- .onFalse(new InstantCommand(() -> spindexer.stopSpindexer()));
+ @Override
+ public double getRawSideTranslation() {
+ return controller.get(LEFT_X);
}
- // Auto shoot
- if (turret != null && hood != null && shooter != null) {
- controller.get(SQUARE).onTrue(
- new InstantCommand(() -> {
- if (autoShoot != null && autoShoot.isScheduled()) {
- autoShoot.cancel();
- } else {
- autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer);
- CommandScheduler.getInstance().schedule(autoShoot);
- }
- }));
+ @Override
+ public double getRawForwardTranslation() {
+ return controller.get(LEFT_Y);
}
- // Climb
+ @Override
+ public double getRawRotation() {
+ return controller.get(RIGHT_X);
+ }
+
+ @Override
+ public double getRawHeadingAngle() {
+ return Math.atan2(controller.get(RIGHT_X), -controller.get(RIGHT_Y)) - Math.PI / 2;
+ }
+
+ @Override
+ public double getRawHeadingMagnitude() {
+ return Math.hypot(controller.get(RIGHT_X), controller.get(RIGHT_Y));
+ }
- // Hood
- if (hood != null) {
- controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> {
- hood.calibrate();
- })).onFalse(new InstantCommand(() -> {
- hood.stopCalibrating();
- }));
+ @Override
+ public boolean getIsSlowMode() {
+ return slowModeSupplier.getAsBoolean();
}
- // Rumble test
- controller.get(RIGHT_JOY).onTrue(new SequentialCommandGroup(
- new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_ON)),
- new WaitCommand(0.5),
- new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF))));
- }
-
- @Override
- public double getRawSideTranslation() {
- return controller.get(LEFT_X);
- }
-
- @Override
- public double getRawForwardTranslation() {
- return controller.get(LEFT_Y);
- }
-
- @Override
- public double getRawRotation() {
- return controller.get(RIGHT_X);
- }
-
- @Override
- public double getRawHeadingAngle() {
- return Math.atan2(controller.get(RIGHT_X), -controller.get(RIGHT_Y)) - Math.PI / 2;
- }
-
- @Override
- public double getRawHeadingMagnitude() {
- return Math.hypot(controller.get(RIGHT_X), controller.get(RIGHT_Y));
- }
-
- @Override
- public boolean getIsSlowMode() {
- return slowModeSupplier.getAsBoolean();
- }
-
- @Override
- public boolean getIsAlign() {
- return false;
- }
-
- public void startRumble() {
- controller.setRumble(GameController.RumbleStatus.RUMBLE_ON);
- }
-
- public void endRumble() {
- controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF);
- }
+ @Override
+ public boolean getIsAlign() {
+ return false;
+ }
+
+ public void startRumble() {
+ controller.setRumble(GameController.RumbleStatus.RUMBLE_ON);
+ }
+
+ public void endRumble() {
+ controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF);
+ }
}
import frc.robot.util.PhoenixOdometryThread;
import lib.CTREModuleState;
-public class Module implements ModuleIO {
- private final ModuleType type;
-
- // Degrees
- private final double angleOffset;
-
- private final TalonFX angleMotor;
- private final TalonFX driveMotor;
- private final CANcoder CANcoder;
- private SwerveModuleState desiredState;
-
- protected boolean stateDeadband = true;
-
- private boolean optimizeStates = true;
-
- // Inputs from drive motor
- private final StatusSignal<Angle> drivePosition;
- private final StatusSignal<AngularVelocity> driveVelocity;
- private final StatusSignal<Voltage> driveAppliedVolts;
- private final StatusSignal<Current> driveCurrent;
-
- // Inputs from turn motor
- private final StatusSignal<Angle> turnAbsolutePosition;
- private final StatusSignal<Angle> turnPosition;
- private final StatusSignal<AngularVelocity> turnVelocity;
- private final StatusSignal<Voltage> turnAppliedVolts;
- private final StatusSignal<Current> turnCurrent;
-
- // Timestamp inputs from Phoenix thread
- protected final Queue<Double> timestampQueue;
- protected final Queue<Double> drivePositionQueue;
- protected final Queue<Double> turnPositionQueue;
-
- private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
-
- // Connection debouncers
- private final Debouncer driveConnectedDebounce = new Debouncer(0.5);
- private final Debouncer turnConnectedDebounce = new Debouncer(0.5);
- private final Debouncer turnEncoderConnectedDebounce = new Debouncer(0.5);
-
- private final Alert driveDisconnectedAlert;
- private final Alert turnDisconnectedAlert;
- private final Alert turnEncoderDisconnectedAlert;
-
- protected final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
-
- private ModuleConstants moduleConstants;
- private final MotionMagicVelocityVoltage velocityRequest = new MotionMagicVelocityVoltage(0.0).withUpdateFreqHz(0);
-
- public Module(ModuleConstants moduleConstants) {
- this.moduleConstants = moduleConstants;
-
- type = moduleConstants.getType();
- angleOffset = moduleConstants.getSteerOffset();
-
- /* Angle Encoder Config */
- CANcoder = new CANcoder(moduleConstants.getEncoderPort(), DriveConstants.STEER_ENCODER_CAN);
- /* Angle Motor Config */
- angleMotor = new TalonFX(moduleConstants.getSteerPort(), DriveConstants.STEER_ENCODER_CAN);
- driveMotor = new TalonFX(moduleConstants.getDrivePort(), DriveConstants.DRIVE_MOTOR_CAN);
- // Create drive status signals
- drivePosition = driveMotor.getPosition();
- driveVelocity = driveMotor.getVelocity();
- driveAppliedVolts = driveMotor.getMotorVoltage();
- driveCurrent = driveMotor.getStatorCurrent();
-
- // Create turn status signals
- turnAbsolutePosition = CANcoder.getAbsolutePosition();
- turnPosition = angleMotor.getPosition();
- turnVelocity = angleMotor.getVelocity();
- turnAppliedVolts = angleMotor.getMotorVoltage();
- turnCurrent = angleMotor.getStatorCurrent();
-
- // Create timestamp queue
- timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
- drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(driveMotor.getPosition());
- turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(angleMotor.getPosition());
- updateInputs();
-
- configCANcoder();
- configAngleMotor();
- configDriveMotor();
-
- driveDisconnectedAlert = new Alert(
- "Disconnected drive motor on module " + Integer.toString(moduleConstants.ordinal()) + ".",
- AlertType.kError);
- turnDisconnectedAlert = new Alert(
- "Disconnected turn motor on module " + Integer.toString(moduleConstants.ordinal()) + ".", AlertType.kError);
- turnEncoderDisconnectedAlert = new Alert(
- "Disconnected turn encoder on module " + Integer.toString(moduleConstants.ordinal()) + ".",
- AlertType.kError);
-
- // Configure periodic frames
- BaseStatusSignal.setUpdateFrequencyForAll(
- 250, drivePosition, turnPosition);
- BaseStatusSignal.setUpdateFrequencyForAll(
- 50.0,
- driveVelocity,
- driveAppliedVolts,
- driveCurrent,
- turnAbsolutePosition,
- turnVelocity,
- turnAppliedVolts,
- turnCurrent);
-
- setDesiredState(new SwerveModuleState(0, getAngle()), false);
- }
-
- public void close() {
- angleMotor.close();
- driveMotor.close();
- CANcoder.close();
- }
-
- @Override
- public void updateInputs() {
- // Refresh all signals
- var driveStatus = BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);
- var turnStatus = BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent);
- var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition);
-
- // Update drive inputs
- inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK());
- inputs.drivePositionRad = Units
- .rotationsToRadians(drivePosition.getValueAsDouble() / DriveConstants.DRIVE_GEAR_RATIO);
- inputs.driveVelocityRadPerSec = Units
- .rotationsToRadians(driveVelocity.getValueAsDouble() / DriveConstants.DRIVE_GEAR_RATIO);
- inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
- inputs.driveCurrentAmps = driveCurrent.getValueAsDouble();
-
- // Update turn inputs
- inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK());
- inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK());
- inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble());
- inputs.turnPosition = Rotation2d
- .fromRotations(turnPosition.getValueAsDouble() / DriveConstants.MODULE_CONSTANTS.angleGearRatio);
- inputs.turnVelocityRadPerSec = Units
- .rotationsToRadians(turnVelocity.getValueAsDouble() / DriveConstants.MODULE_CONSTANTS.angleGearRatio);
- inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble();
- inputs.turnCurrentAmps = turnCurrent.getValueAsDouble();
-
- // Update encoder inputs
- inputs.encoderOffset = inputs.turnAbsolutePosition.getDegrees();
+
+public class Module implements ModuleIO{
+ private final ModuleType type;
+
+ // Degrees
+ private final double angleOffset;
+
+ private final TalonFX angleMotor;
+ private final TalonFX driveMotor;
+ private final CANcoder CANcoder;
+ private SwerveModuleState desiredState;
+
+ protected boolean stateDeadband = true;
+
+ private boolean optimizeStates = true;
+
+ // Inputs from drive motor
+ private final StatusSignal<Angle> drivePosition;
+ private final StatusSignal<AngularVelocity> driveVelocity;
+ private final StatusSignal<Voltage> driveAppliedVolts;
+ private final StatusSignal<Current> driveCurrent;
+
+ // Inputs from turn motor
+ private final StatusSignal<Angle> turnAbsolutePosition;
+ private final StatusSignal<Angle> turnPosition;
+ private final StatusSignal<AngularVelocity> turnVelocity;
+ private final StatusSignal<Voltage> turnAppliedVolts;
+ private final StatusSignal<Current> turnCurrent;
+
+ // Timestamp inputs from Phoenix thread
+ protected final Queue<Double> timestampQueue;
+ protected final Queue<Double> drivePositionQueue;
+ protected final Queue<Double> turnPositionQueue;
+
+ private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
+
+ // Connection debouncers
+ private final Debouncer driveConnectedDebounce = new Debouncer(0.5);
+ private final Debouncer turnConnectedDebounce = new Debouncer(0.5);
+ private final Debouncer turnEncoderConnectedDebounce = new Debouncer(0.5);
+
+ private final Alert driveDisconnectedAlert;
+ private final Alert turnDisconnectedAlert;
+ private final Alert turnEncoderDisconnectedAlert;
+
+ protected final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
+
+ private ModuleConstants moduleConstants;
+ private final MotionMagicVelocityVoltage velocityRequest =
+ new MotionMagicVelocityVoltage(0.0).withUpdateFreqHz(0);
+
+
+ public Module(ModuleConstants moduleConstants) {
+ this.moduleConstants = moduleConstants;
+
+ type = moduleConstants.getType();
+ angleOffset = moduleConstants.getSteerOffset();
+
+ /* Angle Encoder Config */
+ CANcoder = new CANcoder(moduleConstants.getEncoderPort(), DriveConstants.STEER_ENCODER_CAN);
+ /* Angle Motor Config */
+ angleMotor = new TalonFX(moduleConstants.getSteerPort(), DriveConstants.STEER_ENCODER_CAN);
+ driveMotor = new TalonFX(moduleConstants.getDrivePort(), DriveConstants.DRIVE_MOTOR_CAN);
+ // Create drive status signals
+ drivePosition = driveMotor.getPosition();
+ driveVelocity = driveMotor.getVelocity();
+ driveAppliedVolts = driveMotor.getMotorVoltage();
+ driveCurrent = driveMotor.getStatorCurrent();
+
+ // Create turn status signals
+ turnAbsolutePosition = CANcoder.getAbsolutePosition();
+ turnPosition = angleMotor.getPosition();
+ turnVelocity = angleMotor.getVelocity();
+ turnAppliedVolts = angleMotor.getMotorVoltage();
+ turnCurrent = angleMotor.getStatorCurrent();
+
+ // Create timestamp queue
+ timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
+ drivePositionQueue =
+ PhoenixOdometryThread.getInstance().registerSignal(driveMotor.getPosition());
+ turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(angleMotor.getPosition());
+ updateInputs();
+
+ configCANcoder();
+ configAngleMotor();
+ configDriveMotor();
+
+ driveDisconnectedAlert =
+ new Alert(
+ "Disconnected drive motor on module " + Integer.toString(moduleConstants.ordinal()) + ".",
+ AlertType.kError);
+ turnDisconnectedAlert =
+ new Alert(
+ "Disconnected turn motor on module " + Integer.toString(moduleConstants.ordinal()) + ".", AlertType.kError);
+ turnEncoderDisconnectedAlert =
+ new Alert(
+ "Disconnected turn encoder on module " + Integer.toString(moduleConstants.ordinal()) + ".",
+ AlertType.kError);
+
+
+ // Configure periodic frames
+ BaseStatusSignal.setUpdateFrequencyForAll(
+ 250, drivePosition, turnPosition);
+ BaseStatusSignal.setUpdateFrequencyForAll(
+ 50.0,
+ driveVelocity,
+ driveAppliedVolts,
+ driveCurrent,
+ turnAbsolutePosition,
+ turnVelocity,
+ turnAppliedVolts,
+ turnCurrent);
+
+ setDesiredState(new SwerveModuleState(0, getAngle()), false);
+ }
+
+ public void close() {
+ angleMotor.close();
+ driveMotor.close();
+ CANcoder.close();
+ }
+
+ @Override
+ public void updateInputs() {
+ // Refresh all signals
+ var driveStatus =
+ BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);
+ var turnStatus =
+ BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent);
+ var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition);
+
+ // Update drive inputs
+ inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK());
+ inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()/DriveConstants.DRIVE_GEAR_RATIO);
+ inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()/DriveConstants.DRIVE_GEAR_RATIO);
+ inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
+ inputs.driveCurrentAmps = driveCurrent.getValueAsDouble();
+
+ // Update turn inputs
+ inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK());
+ inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK());
+ inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble());
+ inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()/DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()/DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble();
+ inputs.turnCurrentAmps = turnCurrent.getValueAsDouble();
+
+ // Update encoder inputs
+ inputs.encoderOffset = inputs.turnAbsolutePosition.getDegrees();
// Update odometry inputs
- inputs.odometryTimestamps = timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
- inputs.odometryDrivePositionsRad = drivePositionQueue.stream()
- .mapToDouble((Double value) -> Units.rotationsToRadians(value))
- .toArray();
- inputs.odometryTurnPositions = turnPositionQueue.stream()
- .map((Double value) -> Rotation2d.fromRotations(value))
- .toArray(Rotation2d[]::new);
+ inputs.odometryTimestamps =
+ timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
+ inputs.odometryDrivePositionsRad =
+ drivePositionQueue.stream()
+ .mapToDouble((Double value) -> Units.rotationsToRadians(value))
+ .toArray();
+ inputs.odometryTurnPositions =
+ turnPositionQueue.stream()
+ .map((Double value) -> Rotation2d.fromRotations(value))
+ .toArray(Rotation2d[]::new);
timestampQueue.clear();
drivePositionQueue.clear();
turnPositionQueue.clear();
- }
-
- public void periodic() {
- updateInputs();
- Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs);
-
- // Calculate positions for odometry
- int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together
- odometryPositions = new SwerveModulePosition[sampleCount];
- for (int i = 0; i < sampleCount; i++) {
- double positionMeters = inputs.odometryDrivePositionsRad[i] / DriveConstants.DRIVE_GEAR_RATIO
- * DriveConstants.WHEEL_RADIUS;
- Rotation2d angle = inputs.odometryTurnPositions[i].div(DriveConstants.MODULE_CONSTANTS.angleGearRatio);
- odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
- }
- // Update alerts
- driveDisconnectedAlert.set(!inputs.driveConnected);
- turnDisconnectedAlert.set(!inputs.turnConnected);
- turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected);
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("Angle " + moduleConstants.ordinal(), MathUtil.inputModulus(getAngle().getDegrees(), 0, 360));
- }
- }
-
- public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) {
- // Separate if here and in setAngle() to avoid warning
- if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) {
- /*
- * This is a custom optimize function, since default WPILib optimize assumes
- * continuous controller which CTRE and Rev onboard is not
- */
- desiredState = optimizeStates ? CTREModuleState.optimize(wantedState, getState().angle) : wantedState;
- } else {
- desiredState = wantedState;
- }
- setAngle();
- setSpeed(isOpenLoop);
- }
-
- public void setSpeed(boolean isOpenLoop) {
- if (desiredState == null) {
- return;
- }
- if (isOpenLoop) {
- double percentOutput = desiredState.speedMetersPerSecond / DriveConstants.MAX_SPEED;
- driveMotor.setControl(new DutyCycleOut(percentOutput));
- } else {
- double velocity = desiredState.speedMetersPerSecond / DriveConstants.WHEEL_RADIUS / 2 / Math.PI
- * DriveConstants.DRIVE_GEAR_RATIO;
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity);
- }
+ }
+
+ public void periodic() {
+ updateInputs();
+ Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs);
+
+ // Calculate positions for odometry
+ int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together
+ odometryPositions = new SwerveModulePosition[sampleCount];
+ for (int i = 0; i < sampleCount; i++) {
+ double positionMeters = inputs.odometryDrivePositionsRad[i]/DriveConstants.DRIVE_GEAR_RATIO * DriveConstants.WHEEL_RADIUS;
+ Rotation2d angle = inputs.odometryTurnPositions[i].div(DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
+ }
+ // Update alerts
+ driveDisconnectedAlert.set(!inputs.driveConnected);
+ turnDisconnectedAlert.set(!inputs.turnConnected);
+ turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Angle "+ moduleConstants.ordinal(), MathUtil.inputModulus(getAngle().getDegrees(), 0, 360));
+ }
+ }
+
+ public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) {
+ // Separate if here and in setAngle() to avoid warning
+ if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){
+ /*
+ * This is a custom optimize function, since default WPILib optimize assumes
+ * continuous controller which CTRE and Rev onboard is not
+ */
+ desiredState = optimizeStates ? CTREModuleState.optimize(wantedState, getState().angle) : wantedState;
+ }else{
+ desiredState = wantedState;
+ }
+ setAngle();
+ setSpeed(isOpenLoop);
+ }
+
+ public void setSpeed(boolean isOpenLoop) {
+ if(desiredState == null){
+ return;
+ }
+ if (isOpenLoop) {
+ double percentOutput = desiredState.speedMetersPerSecond / DriveConstants.MAX_SPEED;
+ driveMotor.setControl(new DutyCycleOut(percentOutput));
+ } else {
+ double velocity = desiredState.speedMetersPerSecond/DriveConstants.WHEEL_RADIUS/2/Math.PI*DriveConstants.DRIVE_GEAR_RATIO;
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity);
+ }
+
+ double feedforward = velocity * moduleConstants.getDriveV();
+ driveMotor.setControl(
+ velocityRequest
+ .withVelocity(velocity)
+ .withFeedForward(feedforward));
+ }
+ }
+
+ private void setAngle() {
+ if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){
+ // Prevent rotating module if desired speed < 1%. Prevents jittering and unnecessary movement.
+ if (stateDeadband && (Math.abs(desiredState.speedMetersPerSecond) <= (DriveConstants.MAX_SPEED * 0.01))) {
+ stop();
+ return;
+ }
+ }
+ if(desiredState == null){
+ return;
+ }
+ angleMotor.setControl(new PositionDutyCycle(desiredState.angle.getRotations()*DriveConstants.MODULE_CONSTANTS.angleGearRatio));
+ }
+
+ public void setDriveVoltage(Voltage voltage){
+ driveMotor.setControl(new VoltageOut(voltage.baseUnitMagnitude()));
+ }
+ public void setAngle(Rotation2d angle){
+ angleMotor.setControl(new PositionDutyCycle(angle.getRotations()*DriveConstants.MODULE_CONSTANTS.angleGearRatio));
+ }
+
+ public void setOptimize(boolean enable) {
+ optimizeStates = enable;
+ }
+
+ public byte getModuleIndex() {
+ return type.id;
+ }
+
+ public Rotation2d getAngle() {
+ return inputs.turnPosition;
+ }
+
+ public Rotation2d getCANcoder() {
+ return inputs.turnAbsolutePosition;
+ }
+
+ public void resetToAbsolute() {
+ // Sensor ticks
+ double absolutePosition = getCANcoder().getRotations() - Units.degreesToRotations(angleOffset);
+ angleMotor.setPosition(absolutePosition*DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ }
+
+ private void configCANcoder() {
+ CANcoder.getConfigurator().apply(new CANcoderConfiguration());
+ CANcoder.getConfigurator().apply(new MagnetSensorConfigs().withAbsoluteSensorDiscontinuityPoint(1).
+ withSensorDirection(DriveConstants.MODULE_CONSTANTS.canCoderInvert?SensorDirectionValue.Clockwise_Positive:SensorDirectionValue.CounterClockwise_Positive));
+ }
+
+ private void configAngleMotor() {
+ angleMotor.getConfigurator().apply(new TalonFXConfiguration());
+
+ CurrentLimitsConfigs config = new CurrentLimitsConfigs();
+ config.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT;
+ config.SupplyCurrentLimit = DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT;
+ config.SupplyCurrentLowerLimit = DriveConstants.STEER_PEAK_CURRENT_LIMIT;
+ config.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
+ angleMotor.getConfigurator().apply(config);
+ angleMotor.getConfigurator().apply(new Slot0Configs()
+ .withKP(DriveConstants.MODULE_CONSTANTS.angleKP)
+ .withKI(DriveConstants.MODULE_CONSTANTS.angleKI)
+ .withKD(DriveConstants.MODULE_CONSTANTS.angleKD));
+ angleMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_STEER_MOTOR));
+ angleMotor.setNeutralMode(DriveConstants.STEER_NEUTRAL_MODE);
+ angleMotor.setPosition(0);
+
+ // optimize bus utilization for angle motor
+ angleMotor.optimizeBusUtilization();
+
+ resetToAbsolute();
+ }
+
+ /**
+ * @return Speed in RPM
+ */
+ public double getDriveVelocity() {
+ return inputs.driveVelocityRadPerSec*60/DriveConstants.MODULE_CONSTANTS.driveGearRatio/2/Math.PI;
+ }
+
+ public double getDriveVoltage(){
+ return inputs.driveAppliedVolts;
+ }
+
+ public double getDriveStatorCurrent(){
+ return inputs.driveCurrentAmps;
+ }
+
+ // I took the config things straight from this file
+ public void setNewCurrentLimit(double currentSteer, double currentDrive) {
+ CurrentLimitsConfigs steerConfig = new CurrentLimitsConfigs();
+ // steer
+ steerConfig.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT;
+ steerConfig.SupplyCurrentLimit = currentSteer;
+ steerConfig.SupplyCurrentLowerLimit = currentSteer;
+ steerConfig.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
+ angleMotor.getConfigurator().apply(steerConfig); // apply
+
+ // drive
+ CurrentLimitsConfigs driveConfig = new CurrentLimitsConfigs();
+ driveConfig.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+ driveConfig.SupplyCurrentLimit = currentDrive;
+ driveConfig.SupplyCurrentLowerLimit = currentDrive;
+ driveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
+ driveConfig.StatorCurrentLimit = currentDrive;
+ driveConfig.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+ driveMotor.getConfigurator().apply(driveConfig); // apply
+ }
+
+ private void configDriveMotor() {
+ var talonFXConfigs = new TalonFXConfiguration();
+ // set Motion Magic settings
+ var motionMagicConfigs = talonFXConfigs.MotionMagic;
+ motionMagicConfigs.MotionMagicCruiseVelocity = DriveConstants.MAX_SPEED/DriveConstants.WHEEL_CIRCUMFERENCE * DriveConstants.DRIVE_GEAR_RATIO;
+ motionMagicConfigs.MotionMagicAcceleration = DriveConstants.MAX_DRIVE_ACCEL/DriveConstants.WHEEL_CIRCUMFERENCE * DriveConstants.DRIVE_GEAR_RATIO;
+ var slot0Configs = talonFXConfigs.Slot0;
+ slot0Configs.kS = 0; // Add 0.25 V output to overcome static friction
+ slot0Configs.kV = 0.11; // A velocity target of 1 rps results in 0.12 V output
+ slot0Configs.kA = 0.006; // An acceleration of 1 rps/s requires 0.01 V output
+ slot0Configs.kP = moduleConstants.getDriveP(); // A position error of 2.5 rotations results in 12 V output
+ slot0Configs.kI = moduleConstants.getDriveI(); // no output for integrated error
+ slot0Configs.kD = moduleConstants.getDriveD(); // A velocity error of 1 rps results in 0.1 V output
+ driveMotor.getConfigurator().apply(talonFXConfigs);
+ CurrentLimitsConfigs config = new CurrentLimitsConfigs();
+ config.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+ config.SupplyCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
+ config.SupplyCurrentLowerLimit = DriveConstants.DRIVE_PEAK_CURRENT_LIMIT;
+ config.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
+ config.StatorCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
+ config.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+ driveMotor.getConfigurator().apply(config);
+ driveMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_DRIVE_MOTOR));
+ driveMotor.getConfigurator().apply(new OpenLoopRampsConfigs().withDutyCycleOpenLoopRampPeriod(DriveConstants.OPEN_LOOP_RAMP));
+ driveMotor.getConfigurator().apply(new ClosedLoopRampsConfigs().withDutyCycleClosedLoopRampPeriod(DriveConstants.CLOSE_LOOP_RAMP));
+ driveMotor.setNeutralMode(DriveConstants.DRIVE_NEUTRAL_MODE);
+
+ // optimize bus utilization for drive motor
+ driveMotor.optimizeBusUtilization();
+
+ }
+
+ public SwerveModuleState getState() {
+ return new SwerveModuleState(
+ inputs.driveVelocityRadPerSec*DriveConstants.WHEEL_RADIUS,
+ getAngle());
+ }
+
+ public SwerveModulePosition getPosition() {
+ return new SwerveModulePosition(
+ inputs.drivePositionRad*DriveConstants.WHEEL_RADIUS,
+ getAngle());
+ }
+
+ public SwerveModuleState getDesiredState() {
+ return desiredState;
+ }
+
+
+ public double getDriveVelocityError() {
+ return getDesiredState().speedMetersPerSecond - getState().speedMetersPerSecond;
+ }
+
+ public void stop() {
+ driveMotor.set(0);
+ angleMotor.set(0);
+ }
+
+ public ModuleType getModuleType(){
+ return type;
+ }
+
+ public void setStateDeadband(boolean enabled) {
+ stateDeadband = enabled;
+ }
- double feedforward = velocity * moduleConstants.getDriveV();
- driveMotor.setControl(
- velocityRequest
- .withVelocity(velocity)
- .withFeedForward(feedforward));
- }
- }
-
- private void setAngle() {
- if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) {
- // Prevent rotating module if desired speed < 1%. Prevents jittering and
- // unnecessary movement.
- if (stateDeadband && (Math.abs(desiredState.speedMetersPerSecond) <= (DriveConstants.MAX_SPEED * 0.01))) {
- stop();
- return;
+ public double getDesiredVelocity() {
+ return getDesiredState().speedMetersPerSecond;
}
+
+ public Rotation2d getDesiredAngle() {
+ return getDesiredState().angle;
+ }
+
+ /** Returns the module positions received this cycle. */
+ public SwerveModulePosition[] getOdometryPositions() {
+ return odometryPositions;
+ }
+
+ /** Returns the timestamps of the samples received this cycle. */
+ public double[] getOdometryTimestamps() {
+ return inputs.odometryTimestamps;
+ }
+
+ /** returns the drive position status signal for time-synced odometry. */
+ public StatusSignal<Angle> getDrivePositionSignal() {
+ return drivePosition;
+ }
+
+ /** returns the turn position status signal for time-synced odometry. */
+ public StatusSignal<Angle> getTurnPositionSignal() {
+ return turnPosition;
+ }
+
+ /** returns the turn absolute position status signal for time-synced odometry. */
+ public StatusSignal<Angle> getTurnAbsolutePositionSignal() {
+ return turnAbsolutePosition;
+ }
+
+ public TalonFX[] getMotors() {
+ return new TalonFX[]{angleMotor, driveMotor};
}
- if (desiredState == null) {
- return;
- }
- angleMotor.setControl(
- new PositionDutyCycle(desiredState.angle.getRotations() * DriveConstants.MODULE_CONSTANTS.angleGearRatio));
- }
-
- public void setDriveVoltage(Voltage voltage) {
- driveMotor.setControl(new VoltageOut(voltage.baseUnitMagnitude()));
- }
-
- public void setAngle(Rotation2d angle) {
- angleMotor.setControl(new PositionDutyCycle(angle.getRotations() * DriveConstants.MODULE_CONSTANTS.angleGearRatio));
- }
-
- public void setOptimize(boolean enable) {
- optimizeStates = enable;
- }
-
- public byte getModuleIndex() {
- return type.id;
- }
-
- public Rotation2d getAngle() {
- return inputs.turnPosition;
- }
-
- public Rotation2d getCANcoder() {
- return inputs.turnAbsolutePosition;
- }
-
- public void resetToAbsolute() {
- // Sensor ticks
- double absolutePosition = getCANcoder().getRotations() - Units.degreesToRotations(angleOffset);
- angleMotor.setPosition(absolutePosition * DriveConstants.MODULE_CONSTANTS.angleGearRatio);
- }
-
- private void configCANcoder() {
- CANcoder.getConfigurator().apply(new CANcoderConfiguration());
- CANcoder.getConfigurator()
- .apply(new MagnetSensorConfigs().withAbsoluteSensorDiscontinuityPoint(1).withSensorDirection(
- DriveConstants.MODULE_CONSTANTS.canCoderInvert ? SensorDirectionValue.Clockwise_Positive
- : SensorDirectionValue.CounterClockwise_Positive));
- }
-
- private void configAngleMotor() {
- angleMotor.getConfigurator().apply(new TalonFXConfiguration());
-
- CurrentLimitsConfigs config = new CurrentLimitsConfigs();
- config.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT;
- config.SupplyCurrentLimit = DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT;
- config.SupplyCurrentLowerLimit = DriveConstants.STEER_PEAK_CURRENT_LIMIT;
- config.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
- angleMotor.getConfigurator().apply(config);
- angleMotor.getConfigurator().apply(new Slot0Configs()
- .withKP(DriveConstants.MODULE_CONSTANTS.angleKP)
- .withKI(DriveConstants.MODULE_CONSTANTS.angleKI)
- .withKD(DriveConstants.MODULE_CONSTANTS.angleKD));
- angleMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_STEER_MOTOR));
- angleMotor.setNeutralMode(DriveConstants.STEER_NEUTRAL_MODE);
- angleMotor.setPosition(0);
-
- // optimize bus utilization for angle motor
- angleMotor.optimizeBusUtilization();
-
- resetToAbsolute();
- }
-
- /**
- * @return Speed in RPM
- */
- public double getDriveVelocity() {
- return inputs.driveVelocityRadPerSec * 60 / DriveConstants.MODULE_CONSTANTS.driveGearRatio / 2 / Math.PI;
- }
-
- public double getDriveVoltage() {
- return inputs.driveAppliedVolts;
- }
-
- public double getDriveStatorCurrent() {
- return inputs.driveCurrentAmps;
- }
-
- // I took the config things straight from this file
- public void setNewCurrentLimit(double currentSteer, double currentDrive) {
- CurrentLimitsConfigs steerConfig = new CurrentLimitsConfigs();
- // steer
- steerConfig.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT;
- steerConfig.SupplyCurrentLimit = currentSteer;
- steerConfig.SupplyCurrentLowerLimit = currentSteer;
- steerConfig.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
- angleMotor.getConfigurator().apply(steerConfig); // apply
-
- // drive
- CurrentLimitsConfigs driveConfig = new CurrentLimitsConfigs();
- driveConfig.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
- driveConfig.SupplyCurrentLimit = currentDrive;
- driveConfig.SupplyCurrentLowerLimit = currentDrive;
- driveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
- driveConfig.StatorCurrentLimit = currentDrive;
- driveConfig.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
- driveMotor.getConfigurator().apply(driveConfig); // apply
- }
-
- private void configDriveMotor() {
- var talonFXConfigs = new TalonFXConfiguration();
- // set Motion Magic settings
- var motionMagicConfigs = talonFXConfigs.MotionMagic;
- motionMagicConfigs.MotionMagicCruiseVelocity = DriveConstants.MAX_SPEED / DriveConstants.WHEEL_CIRCUMFERENCE
- * DriveConstants.DRIVE_GEAR_RATIO;
- motionMagicConfigs.MotionMagicAcceleration = DriveConstants.MAX_DRIVE_ACCEL / DriveConstants.WHEEL_CIRCUMFERENCE
- * DriveConstants.DRIVE_GEAR_RATIO;
- var slot0Configs = talonFXConfigs.Slot0;
- slot0Configs.kS = 0; // Add 0.25 V output to overcome static friction
- slot0Configs.kV = 0.11; // A velocity target of 1 rps results in 0.12 V output
- slot0Configs.kA = 0.006; // An acceleration of 1 rps/s requires 0.01 V output
- slot0Configs.kP = moduleConstants.getDriveP(); // A position error of 2.5 rotations results in 12 V output
- slot0Configs.kI = moduleConstants.getDriveI(); // no output for integrated error
- slot0Configs.kD = moduleConstants.getDriveD(); // A velocity error of 1 rps results in 0.1 V output
- driveMotor.getConfigurator().apply(talonFXConfigs);
- CurrentLimitsConfigs config = new CurrentLimitsConfigs();
- config.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
- config.SupplyCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
- config.SupplyCurrentLowerLimit = DriveConstants.DRIVE_PEAK_CURRENT_LIMIT;
- config.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
- config.StatorCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
- config.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
- driveMotor.getConfigurator().apply(config);
- driveMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_DRIVE_MOTOR));
- driveMotor.getConfigurator()
- .apply(new OpenLoopRampsConfigs().withDutyCycleOpenLoopRampPeriod(DriveConstants.OPEN_LOOP_RAMP));
- driveMotor.getConfigurator()
- .apply(new ClosedLoopRampsConfigs().withDutyCycleClosedLoopRampPeriod(DriveConstants.CLOSE_LOOP_RAMP));
- driveMotor.setNeutralMode(DriveConstants.DRIVE_NEUTRAL_MODE);
-
- // optimize bus utilization for drive motor
- driveMotor.optimizeBusUtilization();
-
- }
-
- public SwerveModuleState getState() {
- return new SwerveModuleState(
- inputs.driveVelocityRadPerSec * DriveConstants.WHEEL_RADIUS,
- getAngle());
- }
-
- public SwerveModulePosition getPosition() {
- return new SwerveModulePosition(
- inputs.drivePositionRad * DriveConstants.WHEEL_RADIUS,
- getAngle());
- }
-
- public SwerveModuleState getDesiredState() {
- return desiredState;
- }
-
- public double getDriveVelocityError() {
- return getDesiredState().speedMetersPerSecond - getState().speedMetersPerSecond;
- }
-
- public void stop() {
- driveMotor.set(0);
- angleMotor.set(0);
- }
-
- public ModuleType getModuleType() {
- return type;
- }
-
- public void setStateDeadband(boolean enabled) {
- stateDeadband = enabled;
- }
-
- public double getDesiredVelocity() {
- return getDesiredState().speedMetersPerSecond;
- }
-
- public Rotation2d getDesiredAngle() {
- return getDesiredState().angle;
- }
-
- /** Returns the module positions received this cycle. */
- public SwerveModulePosition[] getOdometryPositions() {
- return odometryPositions;
- }
-
- /** Returns the timestamps of the samples received this cycle. */
- public double[] getOdometryTimestamps() {
- return inputs.odometryTimestamps;
- }
-
- /** returns the drive position status signal for time-synced odometry. */
- public StatusSignal<Angle> getDrivePositionSignal() {
- return drivePosition;
- }
-
- /** returns the turn position status signal for time-synced odometry. */
- public StatusSignal<Angle> getTurnPositionSignal() {
- return turnPosition;
- }
-
- /**
- * returns the turn absolute position status signal for time-synced odometry.
- */
- public StatusSignal<Angle> getTurnAbsolutePositionSignal() {
- return turnAbsolutePosition;
- }
-
- public TalonFX[] getMotors() {
- return new TalonFX[] { angleMotor, driveMotor };
- }
}