From: moo Date: Fri, 1 May 2026 15:07:44 +0000 (-0500) Subject: merge X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=refs%2Fheads%2Ftaran-cleanout;p=FRC2026.git merge --- 6b2a5c146674e5f66b86d99e79417c861cf2d084 diff --cc src/main/java/frc/robot/RobotContainer.java index 8d86034,1018db1..b14a229 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@@ -37,8 -34,8 +34,9 @@@ import frc.robot.controls.BaseDriverCon import frc.robot.controls.Operator; import frc.robot.controls.PS5ControllerDriverConfig; import frc.robot.subsystems.Intake.Intake; +import frc.robot.subsystems.Intake.IntakeIOTalonFX; import frc.robot.subsystems.LED.LED; + import frc.robot.subsystems.PowerControl.EMABreaker; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.drivetrain.GyroIOPigeon2; import frc.robot.subsystems.hood.Hood; @@@ -124,14 -104,15 +109,15 @@@ public class RobotContainer default: case PrimeJr: // AKA Valence - spindexer = new Spindexer(); - intake = new Intake(); + spindexer = new Spindexer(new SpindexerIOTalonFX()); + intake = new Intake(new IntakeIOTalonFX()); led = new LED(); + breaker = new EMABreaker(); case WaffleHouse: // AKA Betabot - turret = new Turret(); - shooter = new Shooter(); - hood = new Hood(); + turret = new Turret(new TurretIOTalonFX()); + shooter = new Shooter(new ShooterIOTalonFX()); + hood = new Hood(new HoodIOTalonFX()); case TwinBot: diff --cc src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index b2a9341,6a7caa8..6fbed51 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@@ -30,17 -28,17 +29,17 @@@ import lib.controllers.PS5Controller.PS * 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; + 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( + public PS5ControllerDriverConfig( Drivetrain drive, Shooter shooter, Turret turret, @@@ -56,148 -53,131 +54,119 @@@ 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)); - - // 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; - } - })); + // 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) { + // Spindexer + if (spindexer != null && turret != null && hood != null && intake != null) { - // Toggle spindexer - controller.get(PS5Button.LEFT_TRIGGER).toggleOnTrue( - new RunSpindexer(spindexer, turret, hood, intake) - ); - } + // 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))); - } - } + // 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); + } - 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); + + // 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 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; - } + @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; + } } diff --cc src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java index 84c9293,b5bfdb0..a3f72c3 --- a/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java @@@ -38,204 -38,195 +38,181 @@@ import lib.controllers.GameController.D */ 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; + 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 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; - } + // 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(); + 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); })); - // 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(); - } - })); + // 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(); - })); + } + + // 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())); + } - // 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); - } - })); - } - - // Climb - - // Hood - if (hood != null) { - controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> { - hood.calibrate(); - })).onFalse(new InstantCommand(() -> { - hood.stopCalibrating(); - })); - } + // Auto shoot + if (turret != null && hood != null && shooter != null && spindexer != null) { + autoShoot = new Superstructure(turret, getDrivetrain(), hood, shooter, spindexer); + controller.get(SQUARE).toggleOnTrue(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)))); - } - - @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); - } ++ + // 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); + } } diff --cc src/main/java/frc/robot/subsystems/Intake/Intake.java index c36b889,59f105b..d94069e --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@@ -3,172 -3,405 +3,168 @@@ package frc.robot.subsystems.Intake import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.MotionMagicConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - import edu.wpi.first.math.filter.Debouncer; - import edu.wpi.first.math.filter.Debouncer.DebounceType; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.simulation.ElevatorSim; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color8Bit; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; - import frc.robot.constants.IntakeConstants; -import frc.robot.constants.Constants; -import frc.robot.constants.IdConstants; -public class Intake extends SubsystemBase implements IntakeIO{ - // Mechanism Display... - private final Mechanism2d mechanism; - private final MechanismLigament2d robotExtension; - @SuppressWarnings("unused") - private final MechanismLigament2d robotFrame; - private final MechanismLigament2d robotHeight; - private final MechanismLigament2d robotPos; - - // create the motors - /** Motor to move the roller */ - private TalonFX rollerMotor = new TalonFX(IdConstants.ROLLER_MOTOR_ID, Constants.CANIVORE_SUB); - /** Right motor (master) */ - private TalonFX rightMotor = new TalonFX(IdConstants.RIGHT_MOTOR_ID, Constants.CANIVORE_SUB); - /** Left motor (slave) */ - private TalonFX leftMotor = new TalonFX(IdConstants.LEFT_MOTOR_ID, Constants.CANIVORE_SUB); - - /** Motor characteristics for the roller motor, a single Kraken X44 (aka gearbox) */ - private final DCMotor dcMotorRoller = DCMotor.getKrakenX44(1); - /** Motor characteristics for the extending pair of Kraken X44 motors (aka gearbox) */ - private final DCMotor dcMotorExtend = DCMotor.getKrakenX44(2); - - private double maxVelocity; - private double maxAcceleration; - - // Use FlywheelSim for the roller - private FlywheelSim rollerSim; - - // Use ElevatorSim for the extender - private ElevatorSim intakeSim; - - private final MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0); - - private double setpointInches = 0.0; +public class Intake extends SubsystemBase { - private boolean calibrating = false; - private Debouncer calibrationDebouncer = new Debouncer(0.5, DebounceType.kRising); + private boolean calibrating = false; - private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); - - public Intake() { - - // get the maximum free speed - double maxFreeSpeed = Units.radiansToRotations(dcMotorExtend.freeSpeedRadPerSec)/ IntakeConstants.GEAR_RATIO; - - // max free speed (rot/s) = motor free speed (rad/s to rot/s)/ gear ratio - // safety margin, limits velocity to .75 free speed - maxVelocity = 0.75 * maxFreeSpeed; - maxAcceleration = maxVelocity / 0.25; - - // ----Rollers - // Configure the motors - // Build the configuration for the roller - TalonFXConfiguration rollerConfig = new TalonFXConfiguration(); - - // config Slot 0 PID params - var slot0Configs = rollerConfig.Slot0; - slot0Configs.kP = 5.0; - slot0Configs.kI = 0.0; - slot0Configs.kD = 0.0; - slot0Configs.kV = 0.0; - slot0Configs.kA = 0.0; - - // set the brake mode - rollerConfig.MotorOutput.withNeutralMode(NeutralModeValue.Brake); - - // apply the configuration to the right motor (master) - rollerMotor.getConfigurator().apply(rollerConfig); - - // --- Extenders - - // Build the configuration for the left and right Motor - TalonFXConfiguration config = new TalonFXConfiguration(); - - // config Slot 0 PID params - var extenderSlot0Configs = config.Slot0; - extenderSlot0Configs.kP = 0.5; - extenderSlot0Configs.kI = 0.0; - extenderSlot0Configs.kD = 0.0; - extenderSlot0Configs.kV = 0.0; - extenderSlot0Configs.kA = 0.0; - - // configure MotionMagic - MotionMagicConfigs motionMagicConfigs = config.MotionMagic; - - motionMagicConfigs.MotionMagicCruiseVelocity = IntakeConstants.GEAR_RATIO * maxVelocity/IntakeConstants.RADIUS_RACK_PINION/Math.PI/2; - motionMagicConfigs.MotionMagicAcceleration = IntakeConstants.GEAR_RATIO * maxAcceleration/IntakeConstants.RADIUS_RACK_PINION/Math.PI/2; - - rightMotor.getConfigurator().apply(config); - leftMotor.getConfigurator().apply(config); - - leftMotor.getConfigurator().apply( - new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Coast) - ); - - rightMotor.getConfigurator().apply( - new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast) - ); - - - leftMotor.setPosition(0.0); - rightMotor.setPosition(0.0); - - setNewCurrentLimit(IntakeConstants.STATOR_CURRENT_EXTENDER_LIMIT, IntakeConstants.SUPPLY_CURRENT_EXTENDER_LIMIT, IntakeConstants.STATOR_ROLLER_CURRENT_LIMIT, IntakeConstants.SUPPLY_ROLLER_CURRENT_LIMIT); - - // Build the mechanism for display - mechanism = new Mechanism2d(80, 80); - MechanismRoot2d root = mechanism.getRoot("Root", 0, 1); - robotPos = root.append(new MechanismLigament2d("robotPos", 40, 0.0, 1, new Color8Bit(0, 0, 0))); - robotFrame = robotPos.append(new MechanismLigament2d("Robot Frame",28,0.0, 2, new Color8Bit(0, 255, 255))); - robotHeight = robotPos.append(new MechanismLigament2d("Robot Height", 22.5, 90, 1, new Color8Bit(0,255,255))); - // extensiion is initially retracted. - robotExtension = robotHeight.append(new MechanismLigament2d("Robot Extension", 0, 90, 2, new Color8Bit(255, 0, 0) )); - - // add some test commands. - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putData("Extension Mechanism", mechanism); - SmartDashboard.putData("Intake Calibrate", new InstantCommand(() -> calibrate())); - SmartDashboard.putData("Intake Stop Calibrating", new InstantCommand(() -> stopCalibrating())); - SmartDashboard.putData("Extend Intake", new InstantCommand(() -> extend())); - SmartDashboard.putData("Retract Intake", new InstantCommand(() -> retract())); - } - - if (RobotBase.isSimulation()) { - // Extender simulation - // the supply voltage should change with load.... - rightMotor.getSimState().setSupplyVoltage(12.0); - - // rack pinion is 10 teeth and 10 DP for a radius of 1 inches - double drumRadiusMeters = Units.inchesToMeters(1.0); - double minHeightMeters = Units.inchesToMeters(0.0); - double maxHeightMeters = Units.inchesToMeters(IntakeConstants.MAX_EXTENSION); - // start retracted - double startingHeightMeters = Units.inchesToMeters(0.0); - intakeSim = new ElevatorSim( - dcMotorExtend, - IntakeConstants.GEAR_RATIO, - IntakeConstants.CARRIAGE_MASS_KG, - drumRadiusMeters, - minHeightMeters, - maxHeightMeters, - false, - startingHeightMeters); - - // Roller simulation - rollerSim = new FlywheelSim( - LinearSystemId.createFlywheelSystem(dcMotorRoller, IntakeConstants.ROLLER_MOI_KG_M_SQ, IntakeConstants.ROLLER_GEARING), - dcMotorRoller); - } - } - - public void periodic() { - double inchExtension = getPosition(); - - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("Intake/Setpoint", setpointInches); - } - robotExtension.setLength(inchExtension); - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putNumber("Intake Extension (in)", inchExtension); - SmartDashboard.putBoolean("Intake Extended", inchExtension > 1.0); - } - - if(calibrating){ - leftMotor.set(-0.1); - rightMotor.set(-0.1); - } - - updateInputs(); - Logger.processInputs("Intake", inputs); - - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putBoolean("Intake Calibrated", !calibrating); - SmartDashboard.putBoolean("Intake At Setpoint", Math.abs(inchExtension - setpointInches) < 0.5); - } - } - - public void simulationPeriodic(){ - // get the applied motor voltage - double voltage = rightMotor.getMotorVoltage().getValueAsDouble(); - - // tell the simulator that voltage - intakeSim.setInputVoltage(voltage); - // run the siimulation - intakeSim.update(0.02); - - // get the simulation result - double metersExtend = intakeSim.getPositionMeters(); - double inchesExtend = Units.metersToInches(metersExtend); - double motorRotations = inchesToRotations(inchesExtend); - - // set the motor to that position - rightMotor.getSimState().setRawRotorPosition(motorRotations); - - // update the display - robotExtension.setLength(inchesExtend); - - // simulate roller - voltage = rollerMotor.getMotorVoltage().getValueAsDouble(); - rollerSim.setInputVoltage(voltage); - rollerSim.update(0.020); - - double velocity = Units.radiansToRotations(rollerSim.getAngularVelocityRadPerSec()) * IntakeConstants.ROLLER_GEARING; - - rollerMotor.getSimState().setRotorVelocity(velocity); - } - - /** - * Set the intake extender position - * @param setpoint in inches - */ - public void setPosition(double setpoint) { - double motorRotations = -inchesToRotations(setpoint); - rightMotor.setControl(voltageRequest.withPosition(motorRotations).withEnableFOC(true)); - leftMotor.setControl(voltageRequest.withPosition(motorRotations).withEnableFOC(true)); - - setpointInches = setpoint; - } - - /** - * Get the intake extender position - * @return inches - */ - public double getPosition(){ - return inputs.leftPosition; - } - - /** - * convert rotations to inches - * @param rotations of the motor - * @return inches of rack travel - */ - public double rotationsToInches(double motorRotations) { - // circumference of the rack pinion - double circ = 2 * Math.PI * IntakeConstants.RADIUS_RACK_PINION; - double pinionRotations = motorRotations / IntakeConstants.GEAR_RATIO; - double inches = pinionRotations * circ; - return inches; - } - - /** - * convert inches to rotations - * @param inches of rack travel - * @return motor rotations - */ - public double inchesToRotations(double inches){ - double circ = 2 * Math.PI * IntakeConstants.RADIUS_RACK_PINION; - double pinionRotations = inches / circ; - double motorRotations = pinionRotations * IntakeConstants.GEAR_RATIO; - return motorRotations; - } - - /** - * Set the roller speed. - * @param speed duty cycle in the range [-1, 1] - */ - public void spin(double speed) { - rollerMotor.set(speed); - } - - public double getSpeed() { - return rollerMotor.get(); - } - - /** - * Start the intake roller spinning. - */ - public void spinStart() { - spin(IntakeConstants.SPEED); - } - - /** - * Stop the intake roller. - */ - public void spinStop() { - spin(0.0); - } - - /** - * Reverses the intake roller - */ - public void spinReverse() { - spin(-IntakeConstants.SPEED); - } - - /** Extend the intake the maximum distance. */ - public void extend() { - setPosition(IntakeConstants.MAX_EXTENSION); - } - - /** Extend to a position that doesn't hit the spindexer */ - public void intermediateExtend(){ - setPosition(IntakeConstants.INTERMEDIATE_EXTENSION); - } - - /** Retract the intake to a safe starting position. */ - public void retract(){ - setPosition(IntakeConstants.STOW_EXTENSION); - } - - /** Goes to the zero position */ - public void zeroPosition(){ - setPosition(0.0); - } - - public void zeroMotors() { - rightMotor.setPosition(0.0); - leftMotor.setPosition(0.0); - } - - /** - * Reclaim all the resources (e.g., motors and other devices). - * This step is necessary for multiple unit tests to work. - */ - public void close() { - leftMotor.close(); - rightMotor.close(); - rollerMotor.close(); - } - - /** - * Starts calibrating by running it backwards - */ - public void calibrate(){ - setNewCurrentLimit(IntakeConstants.CALIBRATING_CURRENT_LIMITS, IntakeConstants.CALIBRATING_CURRENT_LIMITS, IntakeConstants.CALIBRATING_CURRENT_LIMITS, IntakeConstants.CALIBRATING_CURRENT_LIMITS); - calibrating = true; - } - - /** - * Stops, zeros, and moves it to retract position - */ - public void stopCalibrating(){ - zeroMotors(); - setNewCurrentLimit(IntakeConstants.STATOR_CURRENT_EXTENDER_LIMIT, IntakeConstants.SUPPLY_CURRENT_EXTENDER_LIMIT, IntakeConstants.STATOR_ROLLER_CURRENT_LIMIT, IntakeConstants.SUPPLY_ROLLER_CURRENT_LIMIT); - calibrating = false; - retract(); - } - - public void setNewCurrentLimit(double statorExtenders, double supplyExtenders, double statorRoller, double supplyRollers) { - CurrentLimitsConfigs limitConfigExtender = new CurrentLimitsConfigs(); - limitConfigExtender.StatorCurrentLimit = statorExtenders; - limitConfigExtender.StatorCurrentLimitEnable = true; - limitConfigExtender.SupplyCurrentLimit = statorExtenders; - limitConfigExtender.SupplyCurrentLimitEnable = true; - leftMotor.getConfigurator().apply(limitConfigExtender); - rightMotor.getConfigurator().apply(limitConfigExtender); - - // roller - CurrentLimitsConfigs limitConfigRoller = new CurrentLimitsConfigs(); - limitConfigRoller.StatorCurrentLimit = statorRoller; - limitConfigRoller.StatorCurrentLimitEnable = true; - limitConfigRoller.SupplyCurrentLimit = supplyRollers; - limitConfigRoller.SupplyCurrentLimitEnable = true; - rollerMotor.getConfigurator().apply(limitConfigRoller); - } - - public double getSubsystemStatorCurrent() { - return inputs.leftStatorCurrent + inputs.rightStatorCurrent + inputs.rollerStatorCurrent; - } - - public double getSubsystemSupplyCurrent() { - return inputs.leftSupplyCurrent + inputs.rightSupplyCurrent + inputs.rollerSupplyCurrent; - } - - @Override - public void updateInputs() { - inputs.leftPosition = rotationsToInches(leftMotor.getPosition().getValueAsDouble()); - inputs.rightPosition = rotationsToInches(rightMotor.getPosition().getValueAsDouble()); - inputs.leftStatorCurrent = leftMotor.getStatorCurrent().getValueAsDouble(); - inputs.rightStatorCurrent = rightMotor.getStatorCurrent().getValueAsDouble(); - inputs.leftSupplyCurrent = leftMotor.getSupplyCurrent().getValueAsDouble(); - inputs.rightSupplyCurrent = rightMotor.getSupplyCurrent().getValueAsDouble(); - inputs.rollerVelocity = rollerMotor.getVelocity().getValueAsDouble(); - inputs.rollerStatorCurrent = rollerMotor.getStatorCurrent().getValueAsDouble(); - inputs.rollerSupplyCurrent = rollerMotor.getSupplyCurrent().getValueAsDouble(); - } - + private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + private final IntakeIO io; + + public Intake(IntakeIO io) { + this.io = io; + } + + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Intake", inputs); + + double inchExtension = inputs.leftPosition; + + if (calibrating) { + io.setRightMotor(-0.1); + io.setLeftMotor(-0.1); + boolean atHardStop = Math + .abs((inputs.leftCurrent + inputs.rightCurrent) + / 2) >= IntakeConstants.CALIBRATING_CURRENT_THRESHOLD; + } + + } + + /** + * convert rotations to inches + * + * @param rotations of the motor + * @return inches of rack travel + */ + public static double rotationsToInches(double motorRotations) { + // circumference of the rack pinion - double circ = 2 * Math.PI * 0.5; ++ double circ = 2 * Math.PI * IntakeConstants.RADIUS_RACK_PINION; + double pinionRotations = motorRotations / IntakeConstants.GEAR_RATIO; + double inches = pinionRotations * circ; + return inches; + } + + /** + * convert inches to rotations + * + * @param inches of rack travel + * @return motor rotations + */ + public static double inchesToRotations(double inches) { - double circ = 2 * Math.PI * 0.5; ++ double circ = 2 * Math.PI * IntakeConstants.RADIUS_RACK_PINION; + double pinionRotations = inches / circ; + double motorRotations = pinionRotations * IntakeConstants.GEAR_RATIO; + return motorRotations; + } + + /** + * Set the roller speed. + * + * @param speed duty cycle in the range [-1, 1] + */ + public void spin(double speed) { + io.setRollerMotor(speed); + } + + public double getSpeed() { + return inputs.rollerSetSpeed; + } + + /** + * Get the intake extender position + * + * @return inches + */ + public double getPosition() { + return inputs.leftPosition; + } + + /** + * Start the intake roller spinning. + */ + public void spinStart() { + spin(IntakeConstants.SPEED); + } + + /** + * Stop the intake roller. + */ + public void spinStop() { + spin(0.0); + } + + /** + * Reverses the intake roller + */ + public void spinReverse() { + spin(-IntakeConstants.SPEED); + } + + /** Extend the intake the maximum distance. */ + public void extend() { + io.setPosition(IntakeConstants.MAX_EXTENSION); + } + + /** Extend to a position that doesn't hit the spindexer */ + public void intermediateExtend() { + io.setPosition(IntakeConstants.INTERMEDIATE_EXTENSION); + } + + /** Retract the intake to a safe starting position. */ + public void retract() { + io.setPosition(IntakeConstants.STOW_EXTENSION); + } + + /** Goes to the zero position */ + public void zeroPosition() { + io.setPosition(0.0); + } + + public void zeroMotors() { + io.setRawPosition(0.0); + } + + /** + * Reclaim all the resources (e.g., motors and other devices). + * This step is necessary for multiple unit tests to work. + */ + public void close() { + io.close(); + } + - /** - * Starts calibrating by running it backwards - */ - public void calibrate() { - setCurrentLimits(IntakeConstants.CALIBRATING_CURRENT_LIMITS); - calibrating = true; - } - - /** - * Stops, zeros, and moves it to retract position - */ - public void stopCalibrating() { - zeroMotors(); - setCurrentLimits(IntakeConstants.EXTENDER_CURRENT_LIMITS); - calibrating = false; - retract(); - } ++// /** ++// * Starts calibrating by running it backwards ++// */ ++// public void calibrate() { ++// setNewCurrentLimit(IntakeConstants.CALIBRATING_CURRENT_LIMITS, IntakeConstants.CALIBRATING_CURRENT_LIMITS, IntakeConstants.CALIBRATING_CURRENT_LIMITS, IntakeConstants.CALIBRATING_CURRENT_LIMITS); ++// calibrating = true; ++// } ++ ++// /** ++// * Stops, zeros, and moves it to retract position ++// */ ++// public void stopCalibrating() { ++// zeroMotors(); ++// setNewCurrentLimit(IntakeConstants.STATOR_CURRENT_EXTENDER_LIMIT, IntakeConstants.SUPPLY_CURRENT_EXTENDER_LIMIT, IntakeConstants.STATOR_ROLLER_CURRENT_LIMIT, IntakeConstants.SUPPLY_ROLLER_CURRENT_LIMIT); ++// calibrating = false; ++// retract(); ++// } + + /** + * sets supply and stator current limits + * + * @param limit the current limit for stator and supply current + */ + public void setCurrentLimits(double limit) { + CurrentLimitsConfigs limits = new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(true) + .withStatorCurrentLimit(limit) + .withSupplyCurrentLimitEnable(true) + .withSupplyCurrentLimit(limit); + + io.setLimits(limits); + } } diff --cc src/main/java/frc/robot/subsystems/Intake/IntakeIOTalonFX.java index c8045ad,0000000..79e575a mode 100644,000000..100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIOTalonFX.java @@@ -1,174 -1,0 +1,173 @@@ +package frc.robot.subsystems.Intake; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import frc.robot.constants.Constants; +import frc.robot.constants.IdConstants; - import frc.robot.constants.IntakeConstants; + +public class IntakeIOTalonFX implements IntakeIO { + + // create the motors + /** Motor to move the roller */ + private TalonFX rollerMotor = new TalonFX(IdConstants.ROLLER_MOTOR_ID, Constants.CANIVORE_SUB); + /** Right motor (master) */ + private TalonFX rightMotor = new TalonFX(IdConstants.RIGHT_MOTOR_ID, Constants.CANIVORE_SUB); + /** Left motor (slave) */ + private TalonFX leftMotor = new TalonFX(IdConstants.LEFT_MOTOR_ID, Constants.CANIVORE_SUB); + + private final MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0); + + /** + * Motor characteristics for the roller motor, a single Kraken X44 (aka gearbox) + */ + private final DCMotor dcMotorRoller = DCMotor.getKrakenX44(1); + /** + * Motor characteristics for the extending pair of Kraken X44 motors (aka + * gearbox) + */ + private final DCMotor dcMotorExtend = DCMotor.getKrakenX44(2); + + public IntakeIOTalonFX() { + + // get the maximum free speed + double maxFreeSpeed = Units.radiansToRotations(dcMotorExtend.freeSpeedRadPerSec) / IntakeConstants.GEAR_RATIO; + + // max free speed (rot/s) = motor free speed (rad/s to rot/s)/ gear ratio + // safety margin, limits velocity to .75 free speed + double maxVelocity = 0.75 * maxFreeSpeed; + double maxAcceleration = maxVelocity / 0.25; + + // Configure the motors + // Build the configuration for the roller + TalonFXConfiguration rollerConfig = new TalonFXConfiguration(); + + // config Slot 0 PID params + var slot0Configs = rollerConfig.Slot0; + slot0Configs.kP = 5.0; + slot0Configs.kI = 0.0; + slot0Configs.kD = 0.0; + slot0Configs.kV = 0.0; + slot0Configs.kA = 0.0; + + // set the brake mode + rollerConfig.MotorOutput.withNeutralMode(NeutralModeValue.Brake); + + // apply the configuration to the right motor (master) + rollerMotor.getConfigurator().apply(rollerConfig); + + // Build the configuration for the left and right Motor + TalonFXConfiguration config = new TalonFXConfiguration(); + + // config the current limits (low value for testing) + config.CurrentLimits - .withStatorCurrentLimit(IntakeConstants.EXTENDER_CURRENT_LIMITS) ++ .withStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_EXTENDER_LIMIT) + .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(IntakeConstants.EXTENDER_CURRENT_LIMITS) ++ .withSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_EXTENDER_LIMIT) + .withSupplyCurrentLimitEnable(true); + + // config Slot 0 PID params + var rollerSlot0Configs = config.Slot0; + rollerSlot0Configs.kP = 5.0; + rollerSlot0Configs.kI = 0.0; + rollerSlot0Configs.kD = 0.0; + rollerSlot0Configs.kV = 0.0; + rollerSlot0Configs.kA = 0.0; + + // configure MotionMagic + MotionMagicConfigs motionMagicConfigs = config.MotionMagic; + + motionMagicConfigs.MotionMagicCruiseVelocity = IntakeConstants.GEAR_RATIO * maxVelocity + / IntakeConstants.RADIUS_RACK_PINION / Math.PI / 2; + motionMagicConfigs.MotionMagicAcceleration = IntakeConstants.GEAR_RATIO * maxAcceleration + / IntakeConstants.RADIUS_RACK_PINION / Math.PI / 2; + + rightMotor.getConfigurator().apply(config); + leftMotor.getConfigurator().apply(config); + + leftMotor.getConfigurator().apply( + new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast)); + + rightMotor.getConfigurator().apply( + new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast)); + + CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); - limitConfig.StatorCurrentLimit = IntakeConstants.NORMAL_CURRENT_LIMIT; ++ limitConfig.StatorCurrentLimit = IntakeConstants.STATOR_ROLLER_CURRENT_LIMIT; + limitConfig.StatorCurrentLimitEnable = true; + leftMotor.getConfigurator().apply(limitConfig); + rightMotor.getConfigurator().apply(limitConfig); + + leftMotor.setPosition(0.0); + rightMotor.setPosition(0.0); + } + + @Override + public void updateInputs(IntakeIOInputs inputs) { + inputs.leftPosition = Intake.rotationsToInches(leftMotor.getPosition().getValueAsDouble()); + inputs.rightPosition = Intake.rotationsToInches(rightMotor.getPosition().getValueAsDouble()); + inputs.leftCurrent = leftMotor.getStatorCurrent().getValueAsDouble(); + inputs.rightCurrent = rightMotor.getStatorCurrent().getValueAsDouble(); + inputs.rollerVelocity = rollerMotor.getVelocity().getValueAsDouble(); + inputs.rollerCurrent = rollerMotor.getStatorCurrent().getValueAsDouble(); + inputs.rightVoltage = rightMotor.getMotorVoltage().getValueAsDouble(); + inputs.leftVoltage = leftMotor.getMotorVoltage().getValueAsDouble(); + inputs.rollerSetSpeed = rollerMotor.get(); + } + + /** + * Set the intake extender position + * + * @param setpoint in inches + */ + @Override + public void setPosition(double setpoint) { + double motorRotations = -Intake.inchesToRotations(setpoint); + rightMotor.setControl(voltageRequest.withPosition(motorRotations).withEnableFOC(true)); + leftMotor.setControl(voltageRequest.withPosition(motorRotations).withEnableFOC(true)); + } + + @Override + public void setLeftMotor(double speed) { + leftMotor.set(speed); + } + + @Override + public void setRightMotor(double speed) { + rightMotor.set(speed); + } + + @Override + public void setRollerMotor(double speed) { + rollerMotor.set(speed); + } + + @Override + public void setLimits(CurrentLimitsConfigs limits) { + leftMotor.getConfigurator().apply(limits); + rightMotor.getConfigurator().apply(limits); + } + + @Override + public void setRawPosition(double pos) { + leftMotor.setPosition(pos); + rightMotor.setPosition(pos); + } + + + @Override + public void close() { + leftMotor.close(); + rightMotor.close(); + rollerMotor.close(); + } + +} diff --cc src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 5b1de6d,0db0422..bf739ad --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@@ -429,11 -429,29 +429,14 @@@ public class Drivetrain extends Subsyst } // for current limit setting (brownout protection) - public void applyNewModuleCurrents(double steerCurrent, double driveCurrent) { + public void applyNewModuleCurrents( + double steerCurrentStator, double steerCurrentSupply, + double driveCurrentStator, double driveCurrentSupply) { for (Module module : modules) { // iterate over our modules - module.setNewCurrentLimit(steerCurrent, driveCurrent); + module.setNewCurrentLimit(steerCurrentStator, steerCurrentSupply, driveCurrentStator, driveCurrentSupply); } } + - public double getSubsystemStatorCurrent() { - double sum = 0; - for (Module module : modules) { - sum += module.getModuleStatorCurrent(); - } - return sum; - } - - public double getSubsystemSupplyCurrent() { - double sum = 0; - for (Module module : modules) { - sum += module.getModuleSupplyCurrent(); - } - return sum; - } /** * Sets the desired states for all swerve modules. diff --cc src/main/java/frc/robot/subsystems/drivetrain/Module.java index 148785c,5511175..2603a69 --- a/src/main/java/frc/robot/subsystems/drivetrain/Module.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Module.java @@@ -21,6 -21,6 +21,7 @@@ import com.ctre.phoenix6.controls.Posit import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.SensorDirectionValue; ++import com.ctre.phoenix6.swerve.jni.SwerveJNI.DriveState; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; @@@ -41,441 -41,450 +42,463 @@@ import frc.robot.constants.swerve.Modul 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 drivePosition; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; - - // Inputs from turn motor - private final StatusSignal turnAbsolutePosition; - private final StatusSignal turnPosition; - private final StatusSignal turnVelocity; - private final StatusSignal turnAppliedVolts; - private final StatusSignal turnCurrent; - - // Timestamp inputs from Phoenix thread - protected final Queue timestampQueue; - protected final Queue drivePositionQueue; - protected final Queue 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 = Units.rotationsToDegrees(CANcoder.getAbsolutePosition().getValueAsDouble()); - - // 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); - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - - inputs.driveStator = driveMotor.getStatorCurrent().getValueAsDouble(); - inputs.driveSupply = driveMotor.getSupplyCurrent().getValueAsDouble(); - inputs.steerStator = angleMotor.getStatorCurrent().getValueAsDouble(); - inputs.steerSupply = angleMotor.getSupplyCurrent().getValueAsDouble(); - } - - 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; - } - - public double getModuleStatorCurrent() { - return inputs.steerStator + inputs.driveStator; - } - - public double getModuleSupplyCurrent() { - return inputs.steerSupply + inputs.driveSupply; - } - - // I took the config things straight from this file - public void setNewCurrentLimit(double currentSteerStator, double currentSteerSupply, double currentDriveStator, double currentDriveSupply) { - CurrentLimitsConfigs steerConfig = new CurrentLimitsConfigs(); - // steer - steerConfig.SupplyCurrentLimitEnable = true; - steerConfig.StatorCurrentLimitEnable = true; - steerConfig.StatorCurrentLimit = currentSteerSupply; - steerConfig.SupplyCurrentLimit = currentSteerSupply; - steerConfig.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION; - angleMotor.getConfigurator().apply(steerConfig); // apply - - // drive - CurrentLimitsConfigs driveConfig = new CurrentLimitsConfigs(); - driveConfig.SupplyCurrentLimitEnable = true; - driveConfig.StatorCurrentLimitEnable = true; - driveConfig.SupplyCurrentLimit = currentDriveSupply; - driveConfig.StatorCurrentLimit = currentDriveStator; - driveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION; - 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 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 drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + // Inputs from turn motor + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnPosition; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + // Timestamp inputs from Phoenix thread + protected final Queue timestampQueue; + protected final Queue drivePositionQueue; + protected final Queue 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); + 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 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 getDrivePositionSignal() { - return drivePosition; - } - - /** returns the turn position status signal for time-synced odometry. */ - public StatusSignal getTurnPositionSignal() { - return turnPosition; - } - - /** returns the turn absolute position status signal for time-synced odometry. */ - public StatusSignal getTurnAbsolutePositionSignal() { - return turnAbsolutePosition; - } - - public TalonFX[] getMotors() { - return new TalonFX[]{angleMotor, driveMotor}; + 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; + } + + 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 getDrivePositionSignal() { + return drivePosition; + } + + /** returns the turn position status signal for time-synced odometry. */ + public StatusSignal getTurnPositionSignal() { + return turnPosition; + } + + /** + * returns the turn absolute position status signal for time-synced odometry. + */ + public StatusSignal getTurnAbsolutePositionSignal() { + return turnAbsolutePosition; + } + + public TalonFX[] getMotors() { + return new TalonFX[] { angleMotor, driveMotor }; + } ++ ++ public void setNewCurrentLimit(double steerCurrentStator, double steerCurrentSupply, double driveCurrentStator, ++ double driveCurrentSupply) { ++ CurrentLimitsConfigs steerConfig = new CurrentLimitsConfigs(); ++ // steer ++ steerConfig.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT; ++ steerConfig.SupplyCurrentLimit = steerCurrentSupply; ++ steerConfig.SupplyCurrentLowerLimit = steerCurrentSupply; ++ steerConfig.StatorCurrentLimit = steerCurrentStator; ++ 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 = driveCurrentSupply; ++ driveConfig.SupplyCurrentLowerLimit = driveCurrentSupply; ++ driveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION; ++ driveConfig.StatorCurrentLimit = driveCurrentStator; ++ driveConfig.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT; ++ driveMotor.getConfigurator().apply(driveConfig); // apply ++ } } diff --cc src/main/java/frc/robot/subsystems/hood/Hood.java index 54bdda1,4bf9d34..ea8fec1 --- a/src/main/java/frc/robot/subsystems/hood/Hood.java +++ b/src/main/java/frc/robot/subsystems/hood/Hood.java @@@ -97,15 -116,16 +97,16 @@@ public class Hood extends SubsystemBas goalVelocityRadPerSec = 0.0; } + double setpointRad = goalAngle.getRadians(); - // calculate shortest angular delta + // calculate shortest angular delta double delta = setpointRad - lastRawSetpoint; delta = MathUtil.angleModulus(delta); - + // filter delta double filteredDelta = setpointFilter.calculate(delta); - + // apply filtered range lastFilteredRad = MathUtil.angleModulus(lastFilteredRad + filteredDelta); lastRawSetpoint = setpointRad; @@@ -122,47 -140,67 +123,32 @@@ // Multiply goal velocity by kV double velocityCompensation = goalVelocityRadPerSec * HoodConstants.FEEDFORWARD_KV; - if (calibrating){ - motor.set(0.1); - boolean atZero = Math.abs(motor.getStatorCurrent().getValueAsDouble()) >= HoodConstants.CALIBRATION_CURRENT_THRESHOLD; + if (calibrating) { + io.setMotorRaw(0.1); + boolean atZero = Math + .abs(inputs.motorCurrent) >= HoodConstants.CALIBRATION_CURRENT_THRESHOLD; boolean calibrated = calibrateDebouncer.calculate(atZero); - if (calibrated) { - if (calibrated){ -- stopCalibrating(); -- } - } else{ + } else { // Set control with feedforward - motor.setControl(mmVoltageRequest - .withPosition(motorGoalRotations) - .withFeedForward(velocityCompensation) - .withEnableFOC(true)); + io.setMotorControl(mmVoltageRequest + .withPosition(motorGoalRotations) + .withFeedForward(velocityCompensation) + .withEnableFOC(true)); } - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("Hood/Voltage", motor.getMotorVoltage().getValue()); - Logger.recordOutput("Hood/velocitySetpoint", goalVelocityRadPerSec / HoodConstants.HOOD_GEAR_RATIO); - Logger.recordOutput("Hood/SetpointDeg", Units.radiansToDegrees(goalAngle.getRadians())); - } - - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putBoolean("Hood Calibrated", !calibrating); - SmartDashboard.putBoolean("Hood At Setpoint", Math.abs(getPositionDeg() - goalAngle.getDegrees()) < 2.0); + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("Hood/Voltage", inputs.motorVoltage); + Logger.recordOutput("Hood/velocitySetpoint", goalVelocityRadPerSec / HoodConstants.HOOD_GEAR_RATIO); + Logger.recordOutput("Hood/SetpointDeg", Units.radiansToDegrees(goalAngle.getRadians())); } } - public void calibrate() { - public void calibrate(){ -- calibrating = true; - setCurrentLimits(HoodConstants.CALIBRATING_CURRENT_LIMIT); - setNewCurrentLimit(HoodConstants.CALIBRATING_CURRENT_LIMIT, HoodConstants.CALIBRATING_CURRENT_LIMIT); -- } -- - public void stopCalibrating() { - io.setPositionRaw(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO); - public void stopCalibrating(){ - motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO); -- goalAngle = new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)); - setCurrentLimits(HoodConstants.NORMAL_CURRENT_LIMIT); - setNewCurrentLimit(HoodConstants.STATOR_CURRENT_LIMIT, HoodConstants.SUPPLY_CURRENT_LIMIT); -- calibrating = false; -- } - - public void setNewCurrentLimit(double stator, double supply) { - CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); - limitConfig.StatorCurrentLimit = stator; - limitConfig.StatorCurrentLimitEnable = true; - limitConfig.SupplyCurrentLimit = supply; - limitConfig.SupplyCurrentLimitEnable = true; - motor.getConfigurator().apply(limitConfig); - } - - public double getSubsystemStatorCurrent() { - return inputs.motorStatorCurrent; - } - - public double getSubsystemSupplyCurrent() { - return inputs.motorSupplyCurrent; - } -- - @Override - public void updateInputs() { - inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / HoodConstants.HOOD_GEAR_RATIO; - inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / HoodConstants.HOOD_GEAR_RATIO; - inputs.motorStatorCurrent = motor.getStatorCurrent().getValueAsDouble(); - inputs.motorSupplyCurrent = motor.getStatorCurrent().getValueAsDouble(); + /** + * sets supply and stator current limits + * + * @param limitAmps the current limit for stator and supply current + */ - public void setCurrentLimits(double limitAmps) { - io.setCurrentLimits(limitAmps); ++ public void setCurrentLimits(double stator, double supply) { ++ io.setCurrentLimits(stator, supply); } } diff --cc src/main/java/frc/robot/subsystems/hood/HoodIO.java index 7e2c91b,ab4e7b3..de9d27f --- a/src/main/java/frc/robot/subsystems/hood/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/hood/HoodIO.java @@@ -2,31 -2,14 +2,29 @@@ package frc.robot.subsystems.hood import org.littletonrobotics.junction.AutoLog; +import com.ctre.phoenix6.controls.MotionMagicVoltage; + public interface HoodIO { -- @AutoLog -- public static class HoodIOInputs{ -- public double positionDeg = HoodConstants.MAX_ANGLE; -- public double velocityRadPerSec = 0.0; - public double motorCurrent = 0.0; - public double motorVoltage = 0.0; - public double motorStatorCurrent = 0.0; - public double motorSupplyCurrent = 0.0; -- } - - public void updateInputs(); ++ @AutoLog ++ public static class HoodIOInputs { ++ public double positionDeg = HoodConstants.MAX_ANGLE; ++ public double velocityRadPerSec = 0.0; ++ public double motorCurrent = 0.0; ++ public double motorVoltage = 0.0; ++ } + - public void updateInputs(HoodIOInputs inputs); ++ public void updateInputs(HoodIOInputs inputs); + - public void setMotorRaw(double speed); ++ public void setMotorRaw(double speed); + - public void setMotorControl(MotionMagicVoltage control); ++ public void setMotorControl(MotionMagicVoltage control); + - public void setPositionRaw(double pos); - - /** - * sets supply and stator current limits - * - * @param limitAmps the current limit for stator and supply current - */ - void setCurrentLimits(double limitAmps); ++ public void setPositionRaw(double pos); + ++ /** ++ * sets supply and stator current limits ++ * ++ * @param limitAmps the current limit for stator and supply current ++ */ ++ public void setCurrentLimits(double stator, double supply); } - diff --cc src/main/java/frc/robot/subsystems/hood/HoodIOTalonFX.java index 2027227,0000000..a03350c mode 100644,000000..100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/hood/HoodIOTalonFX.java @@@ -1,81 -1,0 +1,81 @@@ +package frc.robot.subsystems.hood; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.math.util.Units; +import frc.robot.constants.Constants; +import frc.robot.constants.IdConstants; + +public class HoodIOTalonFX implements HoodIO { + private TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.CANIVORE_SUB); + + public HoodIOTalonFX() { + motor.setNeutralMode(NeutralModeValue.Brake); + + TalonFXConfiguration config = new TalonFXConfiguration(); + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + config.Slot0.kP = 2.0; + config.Slot0.kS = 0.1; // Static friction compensation + config.Slot0.kV = 0.12; // Adjusted kV for the gear ratio + config.Slot0.kD = 0.02; // The "Braking" term to stop overshoot + + var mm = config.MotionMagic; + mm.MotionMagicCruiseVelocity = Units.radiansToRotations(HoodConstants.MAX_VELOCITY) * HoodConstants.HOOD_GEAR_RATIO; + mm.MotionMagicAcceleration = Units.radiansToRotations(HoodConstants.MAX_ACCELERATION) + * HoodConstants.HOOD_GEAR_RATIO; // Lowered for belt safety + mm.MotionMagicJerk = 0; // Set to > 0 for "S-Curve" smoothing if needed + motor.getConfigurator().apply(config); + - setCurrentLimits(HoodConstants.NORMAL_CURRENT_LIMIT); ++ setCurrentLimits(HoodConstants.STATOR_CURRENT_LIMIT, HoodConstants.SUPPLY_CURRENT_LIMIT); + + motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO); + } + + @Override + public void updateInputs(HoodIOInputs inputs) { + inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) + / HoodConstants.HOOD_GEAR_RATIO; + inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) + / HoodConstants.HOOD_GEAR_RATIO; + inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble(); + inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble(); + } + + @Override + public void setMotorRaw(double speed) { + motor.set(speed); + + } + + @Override + public void setMotorControl(MotionMagicVoltage control) { + motor.setControl(control); + } + + @Override + public void setPositionRaw(double pos) { + motor.setPosition(pos); + } + + /** + * sets supply and stator current limits + * + * @param limitAmps the current limit for stator and supply current + */ + @Override - public void setCurrentLimits(double limitAmps) { ++ public void setCurrentLimits(double stator, double supply) { + CurrentLimitsConfigs limits = new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(true) - .withStatorCurrentLimit(limitAmps) ++ .withStatorCurrentLimit(stator) + .withSupplyCurrentLimitEnable(true) - .withSupplyCurrentLimit(limitAmps); ++ .withSupplyCurrentLimit(supply); + + motor.getConfigurator().apply(limits); + } +} diff --cc src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index ffed074,796cf95..a60c5ca --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@@ -3,17 -3,15 +3,19 @@@ package frc.robot.subsystems.shooter import org.littletonrobotics.junction.AutoLog; public interface ShooterIO { - @AutoLog - public static class ShooterIOInputs { - public double shooterSpeedLeft = 0.0; - public double shooterSpeedRight = 0.0; - public double shooterStatorCurrentLeft = 0.0; - public double shooterStatorCurrentRight = 0.0; - public double shooterSupplyCurrentLeft = 0.0; - public double shooterSupplyCurrentRight = 0.0; - } + @AutoLog + public static class ShooterIOInputs { + public double shooterSpeedLeft = 0.0; + public double shooterSpeedRight = 0.0; - public double shooterCurrentLeft = 0.0; - public double shooterCurrentRight = 0.0; ++ public double shooterStatorCurrentLeft = 0.0; ++ public double shooterStatorCurrentRight = 0.0; ++ public double shooterSupplyCurrentLeft = 0.0; ++ public double shooterSupplyCurrentRight = 0.0; + } - public void updateInputs(); + public void updateInputs(ShooterIOInputs inputs); + + public void setNewCurrentLimit(double newCurrentLimit); + + public void setTargetVelocityRps(double target); } diff --cc src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 05faa29,0000000..6eac869 mode 100644,000000..100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@@ -1,78 -1,0 +1,80 @@@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.math.util.Units; +import frc.robot.constants.Constants; +import frc.robot.constants.IdConstants; + +public class ShooterIOTalonFX implements ShooterIO { + + private TalonFX shooterMotorLeft = new TalonFX(IdConstants.SHOOTER_LEFT_ID, Constants.CANIVORE_SUB); + private TalonFX shooterMotorRight = new TalonFX(IdConstants.SHOOTER_RIGHT_ID, Constants.CANIVORE_SUB); + + // TODO Add current limits + + // Velocity in rotations per second + VelocityVoltage voltageRequest = new VelocityVoltage(0); + + public ShooterIOTalonFX() { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Slot0.kP = 0.5; // 0.5 stable + config.Slot0.kI = 0; + config.Slot0.kD = 0.0; + config.Slot0.kV = 0.125; // Maximum rps = 100 --> 12V/100rps + + config.CurrentLimits + .withSupplyCurrentLimit(ShooterConstants.SHOOTER_CURRENT_LIMIT) + .withSupplyCurrentLimitEnable(true); + + shooterMotorLeft.getConfigurator().apply(config); + shooterMotorRight.getConfigurator().apply(config); + + shooterMotorLeft.getConfigurator().apply( + new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast)); + + shooterMotorRight.getConfigurator().apply( + new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast)); + + CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); + limitConfig.StatorCurrentLimit = ShooterConstants.SHOOTER_CURRENT_LIMIT; + limitConfig.StatorCurrentLimitEnable = true; + shooterMotorLeft.getConfigurator().apply(limitConfig); + shooterMotorRight.getConfigurator().apply(limitConfig); + } + + @Override + public void updateInputs(ShooterIOInputs inputs) { + inputs.shooterSpeedLeft = Units.rotationsToRadians(shooterMotorLeft.getVelocity().getValueAsDouble()) + * ShooterConstants.SHOOTER_LAUNCH_DIAMETER / 2; + inputs.shooterSpeedRight = Units.rotationsToRadians(shooterMotorRight.getVelocity().getValueAsDouble()) + * ShooterConstants.SHOOTER_LAUNCH_DIAMETER / 2; - inputs.shooterCurrentLeft = shooterMotorLeft.getStatorCurrent().getValueAsDouble(); - inputs.shooterCurrentRight = shooterMotorRight.getStatorCurrent().getValueAsDouble(); ++ inputs.shooterStatorCurrentLeft = shooterMotorLeft.getStatorCurrent().getValueAsDouble(); ++ inputs.shooterStatorCurrentRight = shooterMotorRight.getStatorCurrent().getValueAsDouble(); ++ inputs.shooterSupplyCurrentLeft = shooterMotorLeft.getSupplyCurrent().getValueAsDouble(); ++ inputs.shooterSupplyCurrentRight = shooterMotorRight.getSupplyCurrent().getValueAsDouble(); + + } + + @Override + public void setNewCurrentLimit(double newCurrentLimit) { + CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); + limitConfig.StatorCurrentLimit = newCurrentLimit; + limitConfig.StatorCurrentLimitEnable = true; + shooterMotorLeft.getConfigurator().apply(limitConfig); + shooterMotorRight.getConfigurator().apply(limitConfig); + } + + @Override + public void setTargetVelocityRps(double target) { + shooterMotorLeft.setControl(voltageRequest.withVelocity(target).withEnableFOC(true)); + shooterMotorRight.setControl(voltageRequest.withVelocity(target).withEnableFOC(true)); + } +} diff --cc src/main/java/frc/robot/subsystems/spindexer/Spindexer.java index 4567eae,6455f15..62aaf80 --- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java @@@ -38,33 -55,26 +38,38 @@@ public class Spindexer extends Subsyste @Override public void periodic() { - updateInputs(); + io.updateInputs(inputs); Logger.processInputs("Spindexer", inputs); + if (resetPos == null) { + io.setPositionRaw(0.1 * gearRatio); + resetPos = (inputs.spindexerOneVelocity / gearRatio) % 1.0; + resetPID.reset(); + } + if (state == SpindexerState.MAX) { - setMotorVoltages(SpindexerConstants.spindexerForwardVoltage); + io.setControl(new DutyCycleOut(SpindexerConstants.spindexerForwardVoltage / 12.0).withEnableFOC(true)); } else if (state == SpindexerState.REVERSE) { - setMotorVoltages(SpindexerConstants.spindexerReverseVoltage); + io.setControl(new DutyCycleOut(SpindexerConstants.spindexerReverseVoltage / 12.0).withEnableFOC(true)); } else if (state == SpindexerState.STOPPED) { - setMotorVoltages(0.0); + io.setControl(new DutyCycleOut(0.0).withEnableFOC(true)); + } else if (state == SpindexerState.RESET && resetPos != null) { + io.setControl(new DutyCycleOut(resetPID.calculate((inputs.spindexerOneVelocity / gearRatio) % 1.0, resetPos)).withEnableFOC(true)); } else { - setMotorVoltages(power); + io.setControl(new DutyCycleOut(power).withEnableFOC(true)); } + if (state == SpindexerState.REVERSE) { - setNewCurrentLimit(SpindexerConstants.SUPPLY_CURRENT_LIMIT, SpindexerConstants.CURRENT_REVERSE_STATOR_LIMIT); ++ setNewCurrentLimit(SpindexerConstants.SUPPLY_CURRENT_LIMIT); + } else { - setNewCurrentLimit(SpindexerConstants.SUPPLY_CURRENT_LIMIT, SpindexerConstants.CURRENT_FORWARD_STATOR_LIMIT); ++ setNewCurrentLimit(SpindexerConstants.SUPPLY_CURRENT_LIMIT); + } + // scale threshold based on power + double velocityThreshold = SpindexerConstants.spindexerVelocityWithBall * power; if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putNumber("Spindexer Ball Count", ballCount); + SmartDashboard.putBoolean("Spindexer Running", state == SpindexerState.MAX || state == SpindexerState.CUSTOM); SmartDashboard.putBoolean("Spindexer Has Ball", ballCount > 0); } @@@ -97,28 -106,31 +102,32 @@@ state = SpindexerState.CUSTOM; } - public void setNewCurrentLimit(double stator, double supply) { - CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); - limitConfig.StatorCurrentLimit = stator; - limitConfig.StatorCurrentLimitEnable = true; - limitConfig.SupplyCurrentLimit = supply; - limitConfig.SupplyCurrentLimitEnable = true; - motorOne.getConfigurator().apply(limitConfig); - motorTwo.getConfigurator().apply(limitConfig); + public void resetSpindexer() { + state = SpindexerState.RESET; } - public double getSubsystemStatorCurrent() { + public void resetResetAngle() { + resetPos = null; + } + + public double getStatorCurrent() { - return inputs.spindexerOneCurrent + inputs.spindexerTwoCurrent; + return inputs.spindexerOneStatorCurrent + inputs.spindexerTwoStatorCurrent; + } + - public double getSubsystemSupplyCurrent() { - return inputs.spindexerOneSupplyCurrent + inputs.spindexerTwoSupplyCurrent; ++ public double getSubsystemStatorCurrent() { ++ return inputs.spindexerOneStatorCurrent + inputs.spindexerTwoStatorCurrent; } - @Override - public void updateInputs() { - inputs.spindexerOneVelocity = motorOne.getVelocity().getValueAsDouble(); - inputs.spindexerOneStatorCurrent = motorOne.getStatorCurrent().getValueAsDouble(); - inputs.spindexerOneSupplyCurrent = motorOne.getSupplyCurrent().getValueAsDouble(); - inputs.spindexerTwoVelocity = motorTwo.getVelocity().getValueAsDouble(); - inputs.spindexerTwoStatorCurrent = motorTwo.getStatorCurrent().getValueAsDouble(); - inputs.spindexerTwoSupplyCurrent = motorTwo.getSupplyCurrent().getValueAsDouble(); + public void setNewCurrentLimit(double newCurrentLimit) { + io.setNewCurrentLimit(newCurrentLimit); } + + + private Double resetPos; + private PIDController resetPID = new PIDController(4.0, 0.0, 0); + + private final double gearRatio = 27.0 / 1.0; //spindexer spins once for every 27 motor spins + + + } diff --cc src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java index f08d5e9,631a8f2..421b1a6 --- a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java +++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java @@@ -6,7 -6,9 +6,10 @@@ public class SpindexerConstants public static final double spindexerForwardVoltage = 1.00; // Volts (set low for testing) public static final double spindexerReverseVoltage = -1.00; // Volts public static final double GEAR_RATIO = 27.0; // unused & both motors have same gearing + public static final double CURRENT_SPIKE_LIMIT = 150.0; + + public static final double CURRENT_FORWARD_STATOR_LIMIT = 150.0; + public static final double CURRENT_REVERSE_STATOR_LIMIT = 20.0; public static final double CURRENT_TIME_LIMIT = 1.0; //s public static final double JAM_CURRENT_THRESHOLD = 75.0; // A public static final double JAM_DEBOUNCE_TIME = 0.3; // seconds diff --cc src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java index 0c8ab05,b1b5b89..d0bf166 --- a/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java +++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java @@@ -8,16 -6,12 +8,18 @@@ public interface SpindexerIO @AutoLog public static class SpindexerIOInputs { public double spindexerOneVelocity = 0.0; - public double spindexerOneCurrent = 0.0; + public double spindexerOneSupplyCurrent = 0.0; + public double spindexerOneStatorCurrent = 0.0; public double spindexerTwoVelocity = 0.0; - public double spindexerTwoCurrent = 0.0; + public double spindexerTwoStatorCurrent = 0.0; + public double spindexerTwoSupplyCurrent = 0.0; } - public void updateInputs(); + public void updateInputs(SpindexerIOInputs inputs); + + public void setControl(ControlRequest request); + + public void setPositionRaw(double pos); + + public void setNewCurrentLimit(double newCurrentLimit); } diff --cc src/main/java/frc/robot/subsystems/spindexer/SpindexerIOTalonFX.java index e2ecefc,0000000..e800a79 mode 100644,000000..100644 --- a/src/main/java/frc/robot/subsystems/spindexer/SpindexerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerIOTalonFX.java @@@ -1,63 -1,0 +1,63 @@@ +package frc.robot.subsystems.spindexer; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; + +import frc.robot.constants.Constants; +import frc.robot.constants.IdConstants; + +public class SpindexerIOTalonFX implements SpindexerIO { + + private TalonFX motorOne = new TalonFX(IdConstants.SPINDEXER_ONE_ID, Constants.CANIVORE_SUB); + private TalonFX motorTwo = new TalonFX(IdConstants.SPINDEXER_TWO_ID, Constants.CANIVORE_SUB); + + public SpindexerIOTalonFX() { + // configure current limit + CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); + limitConfig.StatorCurrentLimit = SpindexerConstants.CURRENT_SPIKE_LIMIT; + limitConfig.StatorCurrentLimitEnable = true; - limitConfig.SupplyCurrentLowerLimit = SpindexerConstants.currentLimit; ++ limitConfig.SupplyCurrentLowerLimit = SpindexerConstants.SUPPLY_CURRENT_LIMIT; + limitConfig.SupplyCurrentLowerTime = 1.5; + motorOne.getConfigurator().apply(limitConfig); + motorTwo.getConfigurator().apply(limitConfig); + + // Invert motor two so they spin in opposite directions + MotorOutputConfigs motorConfig = new MotorOutputConfigs(); + motorConfig.Inverted = InvertedValue.Clockwise_Positive; + motorTwo.getConfigurator().apply(motorConfig); + } + + @Override + public void updateInputs(SpindexerIOInputs inputs) { + inputs.spindexerOneVelocity = motorOne.getVelocity().getValueAsDouble(); - inputs.spindexerOneCurrent = motorOne.getStatorCurrent().getValueAsDouble(); ++ inputs.spindexerOneStatorCurrent = motorOne.getStatorCurrent().getValueAsDouble(); + inputs.spindexerTwoVelocity = motorTwo.getVelocity().getValueAsDouble(); - inputs.spindexerTwoCurrent = motorTwo.getStatorCurrent().getValueAsDouble(); ++ inputs.spindexerTwoStatorCurrent = motorTwo.getStatorCurrent().getValueAsDouble(); + } + + @Override + public void setNewCurrentLimit(double newCurrentLimit) { + CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); + limitConfig.StatorCurrentLimit = newCurrentLimit; + limitConfig.StatorCurrentLimitEnable = true; + limitConfig.SupplyCurrentLowerLimit = newCurrentLimit; + limitConfig.SupplyCurrentLowerTime = 1.5; + motorOne.getConfigurator().apply(limitConfig); + motorTwo.getConfigurator().apply(limitConfig); + } + + @Override + public void setControl(ControlRequest request) { + motorOne.setControl(request); + motorTwo.setControl(request); + } + + @Override + public void setPositionRaw(double pos) { + motorOne.setPosition(pos); + motorTwo.setPosition(pos); + } +} diff --cc src/main/java/frc/robot/subsystems/turret/Turret.java index ad9fd36,d8b595a..538e261 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@@ -20,20 -24,17 +18,18 @@@ import edu.wpi.first.wpilibj.smartdashb import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; - import frc.robot.util.ModifiedCRT; -import frc.robot.constants.IdConstants; -public class Turret extends SubsystemBase implements TurretIO{ +public class Turret extends SubsystemBase { // Super low magnitude filter for the position to make it less jittery private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02, 0.02); - private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged(); + private TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged(); - public boolean locked = false; + private TurretIO io; + + public boolean locked = false; private boolean calibrating; - private Debouncer calibrationDebouncer = new Debouncer(0.5, DebounceType.kRising); /* ---------------- Hardware ---------------- */ @@@ -56,33 -59,53 +52,27 @@@ private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(0); - ModifiedCRT crt; - /* ---------------- Constructor ---------------- */ - public Turret() { - motor.setNeutralMode(NeutralModeValue.Brake); - - TalonFXConfiguration config = new TalonFXConfiguration(); - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - - config.Slot0.kP = 1.5; - config.Slot0.kS = 0.0; // Static friction compensation - config.Slot0.kV = 0.0; // Adjusted kV for the gear ratio - config.Slot0.kD = 0.0; // The "Braking" term to stop overshoot - config.Slot0.kA = 0.0; - - var mm = config.MotionMagic; - mm.MotionMagicCruiseVelocity = Units.radiansToRotations(TurretConstants.MAX_VELOCITY) * TurretConstants.GEAR_RATIO; - mm.MotionMagicAcceleration = Units.radiansToRotations(TurretConstants.MAX_ACCELERATION) * TurretConstants.GEAR_RATIO; // Lowered for belt safety - mm.MotionMagicJerk = 0; // Set to > 0 for "S-Curve" smoothing if needed - motor.getConfigurator().apply(config); - - setNewCurrentLimit(TurretConstants.STATOR_CURRENT_LIMIT, TurretConstants.SUPPLY_CURRENT_LIMIT); + public Turret(TurretIO io) { + this.io = io; - setCurrentLimits(TurretConstants.NORMAL_CURRENT_LIMIT); - lastGoalRad = 0.0; - if (RobotBase.isSimulation()) { - simState = motor.getSimState(); - turretSim = new SingleJointedArmSim( - edu.wpi.first.math.system.plant.DCMotor.getKrakenX60(1), - TurretConstants.GEAR_RATIO, - 0.01, - 0.15, - Units.degreesToRadians(TurretConstants.MIN_ANGLE), - Units.degreesToRadians(TurretConstants.MAX_ANGLE), - false, - 0.0); - } - if (!Constants.DISABLE_SMART_DASHBOARD) { SmartDashboard.putData("Turret Mech", mech); - SmartDashboard.putData("Start turret calibration", new InstantCommand(() -> calibrate())); - SmartDashboard.putData("Stop turret calibration", new InstantCommand(() -> stopCalibrating())); - SmartDashboard.putData("Reset Turret Position to Zero", new InstantCommand(() -> resetTurretPosition())); SendableChooser turretTestChooser = new SendableChooser<>(); - turretTestChooser.setDefaultOption("Turn to 0", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0))); - turretTestChooser.addOption("Turn to -90", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0))); - turretTestChooser.addOption("Turn to 90", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0))); - turretTestChooser.addOption("Turn to 200", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(200), 0.0))); - turretTestChooser.addOption("Turn to -200", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(-200), 0.0))); + turretTestChooser.setDefaultOption("Turn to 0", + new InstantCommand(() -> setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0))); + turretTestChooser.addOption("Turn to -90", + new InstantCommand(() -> setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0))); + turretTestChooser.addOption("Turn to 90", + new InstantCommand(() -> setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0))); + turretTestChooser.addOption("Turn to 200", + new InstantCommand(() -> setFieldRelativeTarget(Rotation2d.fromDegrees(200), 0.0))); + turretTestChooser.addOption("Turn to -200", + new InstantCommand(() -> setFieldRelativeTarget(Rotation2d.fromDegrees(-200), 0.0))); SmartDashboard.putData("Turret Test Positions", turretTestChooser); } @@@ -182,25 -201,16 +172,16 @@@ // Multiply goal velocity by kV double robotTurnCompensation = goalVelocityRadPerSec * TurretConstants.FEEDFORWARD_KV * TurretConstants.GEAR_RATIO; - if (calibrating) { - io.setMotorRaw(0.05); - boolean calibrated = Math - .abs(inputs.motorCurrent) >= TurretConstants.CALIBRATION_CURRENT_THRESHOLD; - if (calibrationDebouncer.calculate(calibrated)) { - stopCalibrating(); - } - } else { - // Sets motor control with feedforward - io.setControl(mmVoltageRequest - .withPosition(motorGoalRotations) - .withFeedForward(robotTurnCompensation) - .withEnableFOC(true)); - } + // Sets motor control with feedforward - motor.setControl(mmVoltageRequest - .withPosition(motorGoalRotations) - .withFeedForward(robotTurnCompensation) - .withEnableFOC(true)); ++ io.setControl(mmVoltageRequest ++ .withPosition(motorGoalRotations) ++ .withFeedForward(robotTurnCompensation) ++ .withEnableFOC(true)); - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue()); + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("Turret/Voltage", inputs.motorVoltage); Logger.recordOutput("Turret/setpointDeg", goalAngle.getDegrees()); - } + } // --- Visualization --- ligament.setAngle(Units.radiansToDegrees(getPositionRad())); @@@ -227,32 -236,29 +208,20 @@@ Units.radiansToRotations(turretSim.getVelocityRadPerSec()) * TurretConstants.GEAR_RATIO); } - @Override - public void updateInputs() { - inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO; - inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / TurretConstants.GEAR_RATIO; - inputs.motorStatorCurrent = motor.getStatorCurrent().getValueAsDouble(); - inputs.motorSupplyCurrent = motor.getSupplyCurrent().getValueAsDouble(); - inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble(); + /** + * sets supply and stator current limits + * + * @param limit the current limit for stator and supply current + */ + public void setCurrentLimits(double limit) { + io.setCurrentLimits(limit); } - public void setNewCurrentLimit(double stator, double supply) { - CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); - limitConfig.StatorCurrentLimit = stator; - limitConfig.StatorCurrentLimitEnable = true; - limitConfig.SupplyCurrentLimit = supply; - limitConfig.SupplyCurrentLimitEnable = true; - motor.getConfigurator().apply(limitConfig); - } - - public double getSubsystemStatorCurrent() { - return inputs.motorStatorCurrent; - } - - public double getSubsystemSupplyCurrent() { - return inputs.motorSupplyCurrent; - } + // Also ignore this for now + private double wrapUnit(double value) { + value %= 1.0; + if (value < 0) + value += 1.0; + return value; + } - - private void calibrate() { - setCurrentLimits(TurretConstants.CALIBRATION_CURRENT_LIMIT); - calibrating = true; - } - - private void stopCalibrating() { - io.setMotorRaw(Units.degreesToRotations(TurretConstants.CALIBRATION_OFFSET) * TurretConstants.GEAR_RATIO); - setCurrentLimits(TurretConstants.NORMAL_CURRENT_LIMIT); - calibrating = false; - setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(TurretConstants.CALIBRATION_OFFSET)), 0.0); - } } diff --cc src/main/java/frc/robot/subsystems/turret/TurretIO.java index 87b3990,385cf19..29f03e0 --- a/src/main/java/frc/robot/subsystems/turret/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIO.java @@@ -2,27 -2,17 +2,30 @@@ package frc.robot.subsystems.turret import org.littletonrobotics.junction.AutoLog; +import com.ctre.phoenix6.controls.ControlRequest; + public interface TurretIO { - @AutoLog - public static class TurretIOInputs{ - public double positionDeg = 0; - public double velocityRadPerSec = 0; - public double motorStatorCurrent = 0; - public double motorSupplyCurrent = 0; - public double encoderLeftRot = 0; - public double encoderRightRot = 0; - public double motorVoltage = 0; - } + @AutoLog + public static class TurretIOInputs { + public double positionDeg = 0; + public double velocityRadPerSec = 0; - public double motorCurrent = 0; ++ public double motorStatorCurrent = 0; ++ public double motorSupplyCurrent = 0; ++ public double encoderLeftRot = 0; ++ public double encoderRightRot = 0; + public double motorVoltage = 0; + } + + public void updateInputs(TurretIOInputs inputs); + + public void setMotorRaw(double speed); + + public void setControl(ControlRequest request); - public void updateInputs(); + /** + * sets supply and stator current limits + * + * @param limit the current limit for stator and supply current + */ + public void setCurrentLimits(double limit); } diff --cc src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 32ba323,0000000..d66c970 mode 100644,000000..100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@@ -1,80 -1,0 +1,87 @@@ +package frc.robot.subsystems.turret; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.math.util.Units; +import frc.robot.constants.Constants; +import frc.robot.constants.IdConstants; + +public class TurretIOTalonFX implements TurretIO { + + private final TalonFX motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.CANIVORE_SUB); + + public TurretIOTalonFX() { + motor.setNeutralMode(NeutralModeValue.Brake); + + TalonFXConfiguration config = new TalonFXConfiguration(); + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + config.Slot0.kP = 12.0; + config.Slot0.kS = 0.1; // Static friction compensation + config.Slot0.kV = 0.12; // Adjusted kV for the gear ratio + config.Slot0.kD = 0.0; // The "Braking" term to stop overshoot + ++ CurrentLimitsConfigs limitsConfigs = new CurrentLimitsConfigs(); ++ limitsConfigs.StatorCurrentLimit = TurretConstants.STATOR_CURRENT_LIMIT; ++ limitsConfigs.SupplyCurrentLimit = TurretConstants.SUPPLY_CURRENT_LIMIT; ++ motor.getConfigurator().apply(limitsConfigs); ++ + var mm = config.MotionMagic; + mm.MotionMagicCruiseVelocity = Units.radiansToRotations(TurretConstants.MAX_VELOCITY) * TurretConstants.GEAR_RATIO; + mm.MotionMagicAcceleration = Units.radiansToRotations(TurretConstants.MAX_ACCELERATION) + * TurretConstants.GEAR_RATIO; // Lowered for belt safety + mm.MotionMagicJerk = 0; // Set to > 0 for "S-Curve" smoothing if needed + motor.getConfigurator().apply(config); + + motor.setPosition(0.0); + } + + @Override + public void updateInputs(TurretIOInputs inputs) { + inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO; + inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) + / TurretConstants.GEAR_RATIO; - inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble(); ++ inputs.motorStatorCurrent = motor.getStatorCurrent().getValueAsDouble(); ++ inputs.motorSupplyCurrent = motor.getSupplyCurrent().getValueAsDouble(); + inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble(); + + } + + @Override + public void setMotorRaw(double speed) { + motor.set(speed); + } + + @Override + public void setControl(ControlRequest request) { + motor.setControl(request); + } + + /** + * sets supply and stator current limits + * + * @param limit the current limit for stator and supply current + */ + @Override + public void setCurrentLimits(double limit) { + CurrentLimitsConfigs limits = new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(true) + .withStatorCurrentLimit(limit) + .withSupplyCurrentLimitEnable(true) + .withSupplyCurrentLimit(limit); + - if (limit == TurretConstants.NORMAL_CURRENT_LIMIT) { - limits.SupplyCurrentLowerLimit = TurretConstants.CALIBRATION_CURRENT_LIMIT; ++ if (limit == TurretConstants.SUPPLY_CURRENT_LIMIT) { ++ limits.SupplyCurrentLowerLimit = TurretConstants.SUPPLY_CURRENT_LIMIT * .5; + limits.SupplyCurrentLowerTime = 1.0; // Set to lower limit if over 1 second + } + + motor.getConfigurator().apply(limits); ++ + } + +}