Limit rotation of the turret

src/main/java/frc/robot/Robot.java

@@ -232,6 +232,7 @@ public void robotPeriodic() {
   }
 
   /** This function is called once when the robot is disabled. */
+  @Override
   public void disabledInit() {}
 
   /** This function is called periodically when disabled. */

src/main/java/frc/robot/subsystems/launcher/Launcher.java

@@ -79,7 +79,7 @@ public void periodic() {
     Logger.processInputs("Flywheel", flywheelInputs);
     Logger.processInputs("Hood", hoodInputs);
 
-    turretDisconnectedAlert.set(!turretInputs.connected);
+    turretDisconnectedAlert.set(!turretInputs.motorControllerConnected);
     flywheelDisconnectedAlert.set(!flywheelInputs.connected);
     hoodDisconnectedAlert.set(!hoodInputs.connected);
 
@@ -141,7 +141,8 @@ public void aim(Translation3d target) {
 
     // Get actual hood & turret position
     Rotation2d hoodPosition = hoodInputs.position;
-    Rotation2d turretPosition = turretInputs.position.plus(turretBasePose.toPose2d().getRotation());
+    Rotation2d turretPosition =
+        turretInputs.relativePosition.plus(turretBasePose.toPose2d().getRotation());
 
     // Build actual velocities
     Translation3d v0_actual =
@@ -180,7 +181,7 @@ public void aim(Translation3d target) {
 
   @AutoLogOutput(key = "Launcher/TurretPose")
   public Pose2d getTurretPose() {
-    return turretBasePose.toPose2d().plus(new Transform2d(0, 0, turretInputs.position));
+    return turretBasePose.toPose2d().plus(new Transform2d(0, 0, turretInputs.relativePosition));
   }
 
   // @AutoLogOutput(key = "Turret/IsOnTarget")

src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java

@@ -30,19 +30,21 @@ public static final class TurretConstants {
     // Geometry
     public static final Transform3d chassisToTurretBase =
         new Transform3d(Inches.of(0), Inches.of(10), Inches.of(22), Rotation3d.kZero);
-    public static final Rotation2d rotationOffset = new Rotation2d(0.44);
+    public static final Rotation2d absEncoderOffset = new Rotation2d(0.5);
+    public static final Rotation2d mechanismOffset = Rotation2d.kCCW_Pi_2;
 
     // Absolute encoder
-    public static final double encoderPositionFactor = 2 * Math.PI; // Radians
-    public static final double encoderVelocityFactor = (2 * Math.PI) / 60.0; // Rad/sec
+    public static final double encoderPositionFactor = (2 * Math.PI) / 5.0; // Radians
+    public static final double encoderVelocityFactor = (2 * Math.PI) / (60.0 * 5.0); // Rad/sec
 
     // Position controller
     public static final double minInput = 0.0;
     public static final double maxInput = 2 * Math.PI;
-    public static final double kPReal = 0.35;
+    public static final double kPReal = 0.5;
 
     // Motor controller
     public static final int port = 12;
+    public static final int DIOPort = 5;
     public static final double motorReduction = 5.0;
     public static final AngularVelocity maxAngularVelocity =
         NEO550Constants.kFreeSpeed.div(motorReduction);

src/main/java/frc/robot/subsystems/launcher/TurretIO.java

@@ -7,11 +7,14 @@
 public interface TurretIO {
   @AutoLog
   public static class TurretIOInputs {
-    public boolean connected = false;
-    public Rotation2d position = Rotation2d.kZero;
+    public boolean motorControllerConnected = false;
+    public Rotation2d relativePosition = Rotation2d.kZero;
     public double velocityRadPerSec = 0.0;
     public double appliedVolts = 0.0;
     public double currentAmps = 0.0;
+
+    public boolean absoluteEncoderConnected = false;
+    public Rotation2d absolutePosition = Rotation2d.kZero;
   }
 
   public default void updateInputs(TurretIOInputs inputs) {}

src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java

@@ -45,8 +45,8 @@ public void updateInputs(TurretIOInputs inputs) {
     turnSim.update(Robot.defaultPeriodSecs);
 
     // Update turn inputs
-    inputs.connected = true;
-    inputs.position = new Rotation2d(turnSim.getAngularPositionRad());
+    inputs.motorControllerConnected = true;
+    inputs.relativePosition = new Rotation2d(turnSim.getAngularPositionRad());
     inputs.velocityRadPerSec = turnSim.getAngularVelocityRadPerSec();
     inputs.appliedVolts = appliedVolts;
     inputs.currentAmps = Math.abs(turnSim.getCurrentDrawAmps());

src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java

@@ -4,38 +4,49 @@
 import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*;
 import static frc.robot.util.SparkUtil.*;
 
-import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
 import com.revrobotics.ResetMode;
-import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.SparkBase;
-import com.revrobotics.spark.SparkBase.ControlType;
-import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
 import frc.robot.Constants.MotorConstants.NEO550Constants;
 import frc.robot.Constants.RobotConstants;
 import java.util.function.DoubleSupplier;
 
 public class TurretIOSpark implements TurretIO {
 
   private final SparkBase turnSpark;
-  private final AbsoluteEncoder turnEncoder;
-  private final SparkClosedLoopController turnController;
+  private final RelativeEncoder turnSparkEncoder;
+  private final PIDController controller;
+  private final DutyCycleEncoder absoluteEncoder;
   private final Debouncer turnConnectedDebounce =
       new Debouncer(0.5, Debouncer.DebounceType.kFalling);
 
+  private boolean closedLoop = false;
+  private double appliedVolts = 0.0;
+  private double feedforwardVolts = 0.0;
+  private boolean seeded = false;
+
   public TurretIOSpark() {
     turnSpark = new SparkMax(port, MotorType.kBrushless);
-    turnEncoder = turnSpark.getAbsoluteEncoder();
-    turnController = turnSpark.getClosedLoopController();
+    turnSparkEncoder = turnSpark.getEncoder();
+    controller = new PIDController(kPReal, 0.0, 0.0);
+    absoluteEncoder =
+        new DutyCycleEncoder(
+            new DigitalInput(DIOPort),
+            2 * Math.PI,
+            absEncoderOffset.getRadians() + mechanismOffset.getRadians());
 
     var turnConfig = new SparkMaxConfig();
 
@@ -46,37 +57,13 @@ public TurretIOSpark() {
         .voltageCompensation(RobotConstants.kNominalVoltage);
 
     turnConfig
-        .absoluteEncoder
-        .inverted(false)
-        .zeroCentered(false)
-        .zeroOffset(rotationOffset.getRotations())
+        .encoder
         .positionConversionFactor(encoderPositionFactor)
-        .velocityConversionFactor(encoderVelocityFactor)
-        .averageDepth(2);
+        .velocityConversionFactor(encoderVelocityFactor);
 
-    turnConfig
-        .closedLoop
-        .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
-        .positionWrappingEnabled(true)
-        .positionWrappingInputRange(0, 2 * Math.PI)
-        .pid(kPReal, 0.0, 0.0);
-
-    // turnConfig
-    //     .softLimit
-    //     .forwardSoftLimitEnabled(false)
-    //     .forwardSoftLimit(maxAngle)
-    //     .reverseSoftLimitEnabled(true)
-    //     .reverseSoftLimit(minAngle);
+    turnConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kPReal, 0.0, 0.0);
 
-    turnConfig
-        .signals
-        .absoluteEncoderPositionAlwaysOn(true)
-        .absoluteEncoderPositionPeriodMs(20)
-        .absoluteEncoderVelocityAlwaysOn(true)
-        .absoluteEncoderVelocityPeriodMs(20)
-        .appliedOutputPeriodMs(20)
-        .busVoltagePeriodMs(20)
-        .outputCurrentPeriodMs(20);
+    turnConfig.signals.appliedOutputPeriodMs(20).busVoltagePeriodMs(20).outputCurrentPeriodMs(20);
 
     tryUntilOk(
         turnSpark,
@@ -88,33 +75,56 @@ public TurretIOSpark() {
 
   @Override
   public void updateInputs(TurretIOInputs inputs) {
+    if (!seeded && absoluteEncoder.isConnected()) {
+      turnSparkEncoder.setPosition(absoluteEncoder.get());
+      seeded = true;
+    }
+
+    // Run closed-loop control
+    if (closedLoop) {
+      appliedVolts = controller.calculate(turnSparkEncoder.getPosition()) + feedforwardVolts;
+    } else {
+      controller.reset();
+    }
+
+    // Update state
+    turnSpark.setVoltage(
+        MathUtil.clamp(
+            appliedVolts, -RobotConstants.kNominalVoltage, RobotConstants.kNominalVoltage));
+
     sparkStickyFault = false;
-    ifOk(turnSpark, turnEncoder::getPosition, (value) -> inputs.position = new Rotation2d(value));
-    ifOk(turnSpark, turnEncoder::getVelocity, (value) -> inputs.velocityRadPerSec = value);
+    ifOk(
+        turnSpark,
+        turnSparkEncoder::getPosition,
+        (value) -> inputs.relativePosition = new Rotation2d(value).plus(mechanismOffset));
+    ifOk(turnSpark, turnSparkEncoder::getVelocity, (value) -> inputs.velocityRadPerSec = value);
     ifOk(
         turnSpark,
         new DoubleSupplier[] {turnSpark::getAppliedOutput, turnSpark::getBusVoltage},
         (values) -> inputs.appliedVolts = values[0] * values[1]);
     ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.currentAmps = value);
-    inputs.connected = turnConnectedDebounce.calculate(!sparkStickyFault);
+    inputs.motorControllerConnected = turnConnectedDebounce.calculate(!sparkStickyFault);
+
+    inputs.absoluteEncoderConnected = absoluteEncoder.isConnected();
+    inputs.absolutePosition = new Rotation2d(absoluteEncoder.get());
   }
 
   @Override
   public void setOpenLoop(double output) {
-    turnSpark.setVoltage(output);
+    closedLoop = false;
+    appliedVolts = output;
   }
 
   @Override
   public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) {
+    closedLoop = true;
     double setpoint =
-        //  Math.max(minAngle, Math.min(maxAngle, rotation.getRadians())) +
-        // rotationOffset.getRadians();
-        MathUtil.inputModulus(rotation.getRadians(), minInput, maxInput);
-    double feedforwardVolts =
+        MathUtil.inputModulus(
+            rotation.getRadians() - mechanismOffset.getRadians(), minInput, maxInput);
+    this.feedforwardVolts =
         RobotConstants.kNominalVoltage
             * angularVelocity.in(RadiansPerSecond)
             / maxAngularVelocity.in(RadiansPerSecond);
-    turnController.setSetpoint(
-        setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardVolts);
+    controller.setSetpoint(setpoint);
   }
 }