turret advantagekit stuff

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 6b05330f677c863fec0b15606cf01d34cb00e57c..3f907b12e3e03ca1d3cb6d7954183403e81eda36 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -2,12 +2,7 @@ package frc.robot.subsystems.turret;
 
 import org.littletonrobotics.junction.Logger;
 
-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 com.ctre.phoenix6.sim.TalonFXSimState;
 
 import edu.wpi.first.math.MathUtil;
@@ -16,7 +11,6 @@ import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import frc.robot.constants.Constants;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
@@ -26,24 +20,23 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.IdConstants;
 import frc.robot.util.ModifiedCRT;
 
-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 ---------------- */
 
-       private final TalonFX motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.CANIVORE_SUB);
-
        private TalonFXSimState simState;
        private SingleJointedArmSim turretSim;
 
@@ -55,8 +48,6 @@ public class Turret extends SubsystemBase implements TurretIO{
        private double lastFilteredRad = 0.0;
        private double lastRawSetpoint = 0.0;
 
-
-
        /* ---------------- Visualization ---------------- */
 
        private final Mechanism2d mech = new Mechanism2d(100, 100);
@@ -70,64 +61,44 @@ public class Turret extends SubsystemBase implements TurretIO{
        /* ---------------- Constructor ---------------- */
 
        public Turret() {
-               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
-
-               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);
 
                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()));
+                       SmartDashboard.putData("Start turret calibration", new InstantCommand(() -> calibrate()));
+                       SmartDashboard.putData("Stop turret calibration", new InstantCommand(() -> stopCalibrating()));
 
                        SendableChooser<InstantCommand> 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);
                }
-               SmartDashboard.putData("Set Locked", new InstantCommand(() -> {locked = !locked;}));
-               //motor.setPosition(Units.degreesToRotations(238.86) * TurretConstants.GEAR_RATIO);
+               SmartDashboard.putData("Set Locked", new InstantCommand(() -> {
+                       locked = !locked;
+               }));
+               // motor.setPosition(Units.degreesToRotations(238.86) *
+               // TurretConstants.GEAR_RATIO);
 
-               motor.setPosition(0.0);
        }
 
        /* ---------------- Public API ---------------- */
 
        /**
-        * Sets the setpoint position and velocity of the turret. Call in command execute.
+        * Sets the setpoint position and velocity of the turret. Call in command
+        * execute.
+        * 
         * @param angle
         * @param velocityRadPerSec
         */
@@ -136,15 +107,12 @@ public class Turret extends SubsystemBase implements TurretIO{
                goalVelocityRadPerSec = velocityRadPerSec;
        }
 
-       public void resetTurretPosition() {
-               inputs.positionDeg = 0.0;
-       }
-
        /**
         * @return If the turret is at setpoint with tolerance of 10 degrees
         */
        public boolean atSetpoint() {
-               if (locked) return true;
+               if (locked)
+                       return true;
                return Math.abs(goalAngle.getRadians() - getPositionRad()) < Units.degreesToRadians(10.0);
        }
 
@@ -152,34 +120,35 @@ public class Turret extends SubsystemBase implements TurretIO{
         * @return Posiiton of the turret in radians
         */
        public double getPositionRad() {
-               return Units.rotationsToRadians(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO;
+               return Units.degreesToRadians(inputs.positionDeg);
        }
 
        /**
         * @return Posiiton of the turret in degrees
         */
        public double getPositionDeg() {
-               return Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO;
+               return inputs.positionDeg;
        }
 
        /* ---------------- Periodic ---------------- */
 
        @Override
        public void periodic() {
-               updateInputs();
+               io.updateInputs(inputs);
                Logger.processInputs("Turret", inputs);
 
                // Position extrapolation
-               double lookAheadSeconds = TurretConstants.EXTRAPOLATION_TIME_CONSTANT; 
-       double futureRobotAngle = goalAngle.getRadians() + (goalVelocityRadPerSec * lookAheadSeconds);
+               double lookAheadSeconds = TurretConstants.EXTRAPOLATION_TIME_CONSTANT;
+               double futureRobotAngle = goalAngle.getRadians() + (goalVelocityRadPerSec * lookAheadSeconds);
 
-               //Continuous wrap selection
+               // Continuous wrap selection
                double best = lastGoalRad;
                boolean found = false;
 
                for (int i = -2; i <= 2; i++) {
                        double candidate = futureRobotAngle + 2.0 * Math.PI * i;
-                       if (candidate < Units.degreesToRadians(TurretConstants.MIN_ANGLE) || candidate > Units.degreesToRadians(TurretConstants.MAX_ANGLE))
+                       if (candidate < Units.degreesToRadians(TurretConstants.MIN_ANGLE)
+                                       || candidate > Units.degreesToRadians(TurretConstants.MAX_ANGLE))
                                continue;
 
                        if (!found || Math.abs(candidate - lastGoalRad) < Math.abs(best - lastGoalRad)) {
@@ -192,10 +161,10 @@ public class Turret extends SubsystemBase implements TurretIO{
 
                // calculate shortest angular delta
                double delta = best - lastRawSetpoint;
-               
+
                // filter delta
                double filteredDelta = setpointFilter.calculate(delta);
-               
+
                // apply filtered range
                lastFilteredRad += filteredDelta;
                lastRawSetpoint = best;
@@ -205,29 +174,32 @@ public class Turret extends SubsystemBase implements TurretIO{
                double motorGoalRotations = Units.radiansToRotations(best) * TurretConstants.GEAR_RATIO;
 
                // Clamp position setpoint to min and max angles
-               motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.degreesToRotations(TurretConstants.MIN_ANGLE) * TurretConstants.GEAR_RATIO, Units.degreesToRotations(TurretConstants.MAX_ANGLE) * TurretConstants.GEAR_RATIO);
-                       
+               motorGoalRotations = MathUtil.clamp(motorGoalRotations,
+                               Units.degreesToRotations(TurretConstants.MIN_ANGLE) * TurretConstants.GEAR_RATIO,
+                               Units.degreesToRotations(TurretConstants.MAX_ANGLE) * TurretConstants.GEAR_RATIO);
+
                // Multiply goal velocity by kV
                double robotTurnCompensation = goalVelocityRadPerSec * TurretConstants.FEEDFORWARD_KV * TurretConstants.GEAR_RATIO;
 
-               if(calibrating){
-                       motor.set(0.05);
-                       boolean calibrated = Math.abs(motor.getStatorCurrent().getValueAsDouble()) >= TurretConstants.CALIBRATION_CURRENT_THRESHOLD;
-                       if(calibrationDebouncer.calculate(calibrated)){
+               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
-                       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()));
@@ -237,14 +209,14 @@ public class Turret extends SubsystemBase implements TurretIO{
                        SmartDashboard.putBoolean("Turret Calibrated", !calibrating);
                        SmartDashboard.putBoolean("Turret At Setpoint", atSetpoint());
                }
-               
+
        }
 
        /* ---------------- Simulation ---------------- */
 
        @Override
        public void simulationPeriodic() {
-               turretSim.setInputVoltage(motor.getMotorVoltage().getValueAsDouble());
+               turretSim.setInputVoltage(inputs.motorVoltage);
                turretSim.update(Constants.LOOP_TIME);
 
                simState.setRawRotorPosition(
@@ -254,47 +226,30 @@ public class Turret extends SubsystemBase implements TurretIO{
                                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.motorCurrent = motor.getStatorCurrent().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) {
-        CurrentLimitsConfigs limits = new CurrentLimitsConfigs()
-        .withStatorCurrentLimitEnable(true)
-        .withStatorCurrentLimit(limit)
-        .withSupplyCurrentLimitEnable(true)
-        .withSupplyCurrentLimit(limit);
-
-               if(limit == TurretConstants.NORMAL_CURRENT_LIMIT){
-                       limits.SupplyCurrentLowerLimit = TurretConstants.CALIBRATION_CURRENT_LIMIT;
-                       limits.SupplyCurrentLowerTime = 1.0; // Set to lower limit if over 1 second
-               }
-
-        motor.getConfigurator().apply(limits);
-    }
+        * sets supply and stator current limits
+        * 
+        * @param limit the current limit for stator and supply current
+        */
+       public void setCurrentLimits(double limit) {
+               io.setCurrentLimits(limit);
+       }
 
        // Also ignore this for now
        private double wrapUnit(double value) {
                value %= 1.0;
-               if (value < 0) value += 1.0;
+               if (value < 0)
+                       value += 1.0;
                return value;
        }
 
-       private void calibrate(){
+       private void calibrate() {
                setCurrentLimits(TurretConstants.CALIBRATION_CURRENT_LIMIT);
                calibrating = true;
        }
 
-       private void stopCalibrating(){
-               motor.set(Units.degreesToRotations(TurretConstants.CALIBRATION_OFFSET) * TurretConstants.GEAR_RATIO);
+       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 --git a/src/main/java/frc/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/turret/TurretIO.java
index a7bd928366220e9cd9d7600234c91516ebd04cc9..87b39904862c4d77a8f2ef6681ff84603c3b6d1d 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/TurretIO.java
+++ b/src/main/java/frc/robot/subsystems/turret/TurretIO.java
@@ -2,16 +2,27 @@ 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 motorCurrent = 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 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);
 }