Merge pull request #168 from thedropbears/enforce-climb-state

Lock the other mechanisms when climbing

components/climber.py

@@ -33,42 +33,24 @@ def __init__(self) -> None:
         self.climb_encoder = self.climbing_motor.getEncoder()
         self.climb_encoder.setPositionConversionFactor(self.GEAR_RATIO)
 
-        self.encoder_limit_enabled = False
-        self.climbing_motor.setSoftLimit(
-            CANSparkMax.SoftLimitDirection.kForward, self.SHAFT_REV_TOP_LIMIT
-        )
-        self.climbing_motor.setSoftLimit(
-            CANSparkMax.SoftLimitDirection.kReverse, self.SHAFT_REV_BOTTOM_LIMIT
-        )
-        self.climbing_motor.enableSoftLimit(
-            CANSparkMax.SoftLimitDirection.kForward, False
-        )
-        self.climbing_motor.enableSoftLimit(
-            CANSparkMax.SoftLimitDirection.kReverse, False
-        )
         self.last_position = self.POSITION.RETRACTED
 
+        self.seen_deploy_limit_switch = False
+
+    def on_disable(self) -> None:
+        self.seen_deploy_limit_switch = False
+
     @feedback
     def has_climb_finished(self) -> bool:
-        return not self.retract_limit_switch.get() or (
-            self.encoder_limit_enabled
-            and self.climbing_motor.getFault(CANSparkMax.FaultID.kSoftLimitRev)
-        )
+        return not self.retract_limit_switch.get()
 
     @feedback
     def has_deploy_finished(self) -> bool:
-        return not self.deploy_limit_switch.get() or (
-            self.encoder_limit_enabled
-            and self.climbing_motor.getFault(CANSparkMax.FaultID.kSoftLimitFwd)
-        )
+        return not self.deploy_limit_switch.get()
 
-    def enable_soft_limits(self) -> None:
-        self.climbing_motor.enableSoftLimit(
-            CANSparkMax.SoftLimitDirection.kForward, True
-        )
-        self.climbing_motor.enableSoftLimit(
-            CANSparkMax.SoftLimitDirection.kReverse, True
-        )
+    def should_lock_mechanisms(self) -> bool:
+        # Climbs in the last 20 seconds are real climbs...
+        return self.seen_deploy_limit_switch
 
     def deploy(self) -> None:
         if self.has_deploy_finished():
@@ -83,22 +65,21 @@ def retract(self) -> None:
             self.speed = -1.0
 
     def execute(self) -> None:
-        if not self.encoder_limit_enabled:
-            if self.has_climb_finished():
-                self.enable_soft_limits()
-                self.encoder_limit_enabled = True
-                self.climb_encoder.setPosition(self.SHAFT_REV_BOTTOM_LIMIT)
+        if self.has_climb_finished():
+            self.climb_encoder.setPosition(self.SHAFT_REV_BOTTOM_LIMIT)
 
-            if self.has_deploy_finished():
-                self.enable_soft_limits()
-                self.encoder_limit_enabled = True
-                self.climb_encoder.setPosition(self.SHAFT_REV_TOP_LIMIT)
+        if self.has_deploy_finished():
+            self.climb_encoder.setPosition(self.SHAFT_REV_TOP_LIMIT)
 
         if self.has_climb_finished():
             if self.last_position is not self.POSITION.RETRACTED:
                 self.status_lights.climbing_arm_retracted()
                 self.last_position = self.POSITION.RETRACTED
+            if wpilib.DriverStation.getMatchTime() > 20:
+                # reset in case of accidental climb
+                self.seen_deploy_limit_switch = False
         elif self.has_deploy_finished():
+            self.seen_deploy_limit_switch = True
             if self.last_position is not self.POSITION.DEPLOYED:
                 self.status_lights.climbing_arm_fully_extended()
                 self.last_position = self.POSITION.DEPLOYED

components/intake.py

@@ -148,6 +148,14 @@ def __init__(self) -> None:
         self.has_indexed = False
         self.stall_detection_enabled = False
 
+        self.locked = False
+
+    def lock(self) -> None:
+        self.locked = True
+
+    def unlock(self) -> None:
+        self.locked = False
+
     @feedback
     def _at_retract_hard_limit(self) -> bool:
         return self.retract_limit_switch.get()
@@ -241,6 +249,12 @@ def execute(self) -> None:
         elif self.motor.get_velocity().value > self.INTAKE_RUNNING_VELOCITY:
             self.stall_detection_enabled = True
 
+        # lock the component if climbing or finished a real climb
+        if self.locked:
+            self.retract()
+            self.direction = self.Direction.STOPPED
+            self.desired_injector_speed = 0.0
+
         intake_request = VoltageOut(self.direction.value * self.motor_speed * 12.0)
 
         self.motor.set_control(intake_request)

components/shooter.py

@@ -66,6 +66,7 @@ class ShooterComponent:
     desired_flywheel_speed = tunable(0.0)
 
     def __init__(self) -> None:
+        self.locked = False
         self.inclinator = CANSparkMax(
             SparkMaxIds.shooter_inclinator, CANSparkMax.MotorType.kBrushless
         )
@@ -132,6 +133,12 @@ def get_applied_output(self) -> float:
     def on_enable(self) -> None:
         self.inclinator_controller.reset()
 
+    def lock(self) -> None:
+        self.locked = True
+
+    def unlock(self) -> None:
+        self.locked = False
+
     @feedback
     def is_ready(self) -> bool:
         """Is the shooter ready to fire?"""
@@ -205,6 +212,10 @@ def execute(self) -> None:
         )
         self.inclinator.set(inclinator_speed)
 
+        # stop the flywheels while climbing or permenantly after a real climb
+        if self.locked:
+            self.desired_flywheel_speed = 0
+
         if self.desired_flywheel_speed == 0:
             self.flywheel_left.set_control(NeutralOut())
         else:

robot.py

@@ -74,6 +74,13 @@ def teleopInit(self) -> None:
         pass
 
     def teleopPeriodic(self) -> None:
+        if self.climber.should_lock_mechanisms():
+            self.shooter_component.lock()
+            self.intake_component.lock()
+        else:
+            self.shooter_component.unlock()
+            self.intake_component.unlock()
+
         # Set max speed
         max_speed = self.max_speed
         max_spin_rate = self.max_spin_rate