Add clamping to hood and turret in place of state control

Author: SPUD42AG

src/main/java/com/spartronics4915/frc2026/subsystems/mechanisms/ClimberSubsystem.java

@@ -45,7 +45,6 @@ public ClimberSubsystem() {
             motorConfig.apply(motorOutputConfigs);
 
         setMechanismPosition(0);
-
         ModeSwitchHandler.EnableModeSwitchHandler(this);
     }
 

src/main/java/com/spartronics4915/frc2026/subsystems/mechanisms/head/HoodSubsystem.java

@@ -7,6 +7,7 @@
 import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
+import com.spartronics4915.frc2026.subsystems.mechanisms.head.TurretSubsystem.TurretClamp;
 import com.spartronics4915.frc2026.util.ModeSwitchHandler;
 import com.spartronics4915.frc2026.util.ModeSwitchHandler.ModeSwitchInterface;
 
@@ -28,13 +29,17 @@ public class HoodSubsystem extends SubsystemBase implements ModeSwitchInterface
 
     TalonFX motor = new TalonFX(MOTOR_ID);
 
-    TrapezoidProfile trapProfile = new TrapezoidProfile(
+    TrapezoidProfile trapezoidProfile = new TrapezoidProfile(
 	    new Constraints(MAX_VELOCITY, MAX_ACCELERATION)
     );
 
     private Rotation2d currentSetpoint = Rotation2d.fromDegrees(0);
     private State currentState = new State();
 
+    private HoodClamp currentClamp;
+    private Rotation2d minAngle;
+    private Rotation2d maxAngle;
+
     private final DoublePublisher appliedOutPublisher = NetworkTableInstance.getDefault().getTable("hood").getDoubleTopic("applied out").publish();
     private final StructPublisher<Rotation2d> positionPublisher = NetworkTableInstance.getDefault().getTable("hood").getStructTopic("position", Rotation2d.struct).publish();
     private final StructPublisher<Rotation2d> desiredStatePublisher = NetworkTableInstance.getDefault().getTable("hood").getStructTopic("desiredState", Rotation2d.struct).publish();
@@ -50,13 +55,12 @@ public HoodSubsystem() {
             motorOutputConfigs.Inverted = InvertedValue.Clockwise_Positive;
             motorConfig.apply(motorOutputConfigs);
 
-        setMechanismAngle(Rotation2d.fromDegrees(0));
+        currentClamp = HoodClamp.RESTRICTED;
+            minAngle = currentClamp.minAngle;
+            maxAngle = currentClamp.maxAngle;
 
+        setMechanismAngle(Rotation2d.fromDegrees(0));
         ModeSwitchHandler.EnableModeSwitchHandler(this);
-
-        SmartDashboard.putData("Hood Up", setStateCommand(HoodState.UP));
-        SmartDashboard.putData("Hood Middle", setStateCommand(HoodState.MIDDLE));
-        SmartDashboard.putData("Hood Down", setStateCommand(HoodState.DOWN));
     }
 
     //#region Main Functionality
@@ -66,12 +70,12 @@ public void periodic(){
         currentSetpoint = Rotation2d.fromRotations(
             MathUtil.clamp(
                 currentSetpoint.getRotations(), 
-                MIN_ANGLE.getRotations(), 
-                MAX_ANGLE.getRotations()
+                minAngle.getRotations(), 
+                maxAngle.getRotations()
             )
         );
 
-        currentState = trapProfile.calculate(
+        currentState = trapezoidProfile.calculate(
             DELTA_TIME, 
             currentState, 
             new State(currentSetpoint.getRotations(), 0.0)
@@ -95,8 +99,10 @@ public void setSetpoint(Rotation2d setpoint){
         currentSetpoint = setpoint;
     }
 
-    public void setState(HoodState state){
-        currentSetpoint = state.angle;
+    public void setClamp(HoodClamp clamp){
+        currentClamp = clamp;
+            minAngle = currentClamp.minAngle;
+            maxAngle = currentClamp.maxAngle;
     }
 
     private void setMechanismAngle(Rotation2d angle){
@@ -120,20 +126,17 @@ public void resetMechanism(Rotation2d angle){
     public Command setSetpointCommand(Rotation2d newSetpoint){
         return this.runOnce(() -> setSetpoint(newSetpoint));
     }
-
-    public Command setStateCommand(HoodState state){
-        return setSetpointCommand(state.angle);
-    }
 
-    public enum HoodState {
-        DOWN(Rotation2d.fromDegrees(0)),
-        UP(Rotation2d.fromDegrees(30)),
-        MIDDLE(Rotation2d.fromDegrees(15));
+    public enum HoodClamp {
+        RESTRICTED(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0)),
+        UNRESTRICTED(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0));
 
-        Rotation2d angle;
+        Rotation2d minAngle;
+        Rotation2d maxAngle;
 
-        private HoodState(Rotation2d angle) {
-            this.angle = angle;
+        private HoodClamp(Rotation2d minAngle, Rotation2d maxAngle) {
+            this.minAngle = minAngle;
+            this.maxAngle = maxAngle;
         }
     }
 

src/main/java/com/spartronics4915/frc2026/subsystems/mechanisms/head/TurretSubsystem.java

@@ -33,7 +33,7 @@ public class TurretSubsystem extends SubsystemBase implements ModeSwitchInterfac
     private Rotation2d currentSetpoint = Rotation2d.fromDegrees(0);
     private State currentState = new State();
 
-    private TurretState currentTurretState;
+    private TurretClamp currentClamp;
     private Rotation2d minAngle;
     private Rotation2d maxAngle;
 
@@ -48,9 +48,9 @@ public TurretSubsystem(){
             configurator.apply(CURRENT_LIMITS_CONFIG);
             configurator.apply(FEEDBACK_CONFIG);
 
-        currentTurretState = TurretState.RESTRICTED;
-            minAngle = currentTurretState.minAngle;
-            maxAngle = currentTurretState.maxAngle;
+        currentClamp = TurretClamp.RESTRICTED;
+            minAngle = currentClamp.minAngle;
+            maxAngle = currentClamp.maxAngle;
 
         setMechanismAngle(Rotation2d.fromDegrees(0));
         ModeSwitchHandler.EnableModeSwitchHandler(this);
@@ -92,10 +92,10 @@ public void setSetpoint(Rotation2d setpoint){
         currentSetpoint = setpoint;
     }
 
-    public void setTurretState(TurretState state){
-        currentTurretState = state;
-            minAngle = currentTurretState.minAngle;
-            maxAngle = currentTurretState.maxAngle;
+    public void setClamp(TurretClamp clamp){
+        currentClamp = clamp;
+            minAngle = currentClamp.minAngle;
+            maxAngle = currentClamp.maxAngle;
     }
 
     private void setMechanismAngle(Rotation2d angle){
@@ -122,14 +122,14 @@ public Command setSetpointCommand(Rotation2d newSetpoint){
 
     //#endregion
 
-    public enum TurretState {
+    public enum TurretClamp {
         RESTRICTED(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0)),
         UNRESTRICTED(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0));
 
         Rotation2d minAngle;
         Rotation2d maxAngle;
 
-        private TurretState(Rotation2d minAngle, Rotation2d maxAngle) {
+        private TurretClamp(Rotation2d minAngle, Rotation2d maxAngle) {
             this.minAngle = minAngle;
             this.maxAngle = maxAngle;
         }

src/main/java/com/spartronics4915/frc2026/subsystems/mechanisms/pipeline/ShooterSubsystem.java

@@ -55,7 +55,6 @@ public ShooterSubsystem() {
         followerMotor.setControl(new Follower(FOLLOWER_MOTOR_ID, MotorAlignmentValue.Aligned));
 
         leadMotor.set(0);
-
         ModeSwitchHandler.EnableModeSwitchHandler(this);
 
         SmartDashboard.putData("Shooter On", setStateCommand(ShooterState.ON));