Destroy swerve module

rio/commands/climb.py

@@ -1,3 +1,5 @@
+import typing
+import logging
 import commands2
 
 from constants import ClimberConstants
@@ -7,7 +9,15 @@
 
 
 class Climb(commands2.Command):
-    def __init__(self, speed, _climber: Climber, _shooter: Shooter):
+    def __init__(
+        self,
+        _speed: typing.Callable[
+            [],
+            float,
+        ],
+        _climber: Climber,
+        _shooter: Shooter,
+    ):
         """
         rotates the climber
         """
@@ -18,20 +28,28 @@ def __init__(self, speed, _climber: Climber, _shooter: Shooter):
         self.shooter = _shooter
 
         # local var of speed
-        self.speed = speed
+        self.speed = _speed
 
         # Command requires
         self.addRequirements((self.climber, self.shooter))
 
     def execute(self):
-        if self.speed >= 0:
+        logging.debug(f"Climb is {self.speed()}")
+
+        if self.speed() == 0 or self.speed() >= 0.5:
             # Positive numbers imply we're pulling DOWN
             self.climber.setServoAngle(ClimberConstants.kServoLock)
+            self.shooter.rotateManual(
+                -self.speed() * ClimberConstants.kClimberShooterForward
+            )
         else:
             # Negative numbers imply we're relaxing
             self.climber.setServoAngle(ClimberConstants.kservoOpen)
+            self.shooter.rotateManual(
+                -self.speed() * ClimberConstants.kClimberShooterBackward
+            )
 
-        self.climber.setClimberMotorSpeed(self.speed)
+        self.climber.setClimberMotorSpeed(self.speed())
 
     def end(self, interrupted: bool):
         self.climber.setClimberMotorSpeed(0)

rio/commands/shooterROT.py

@@ -22,7 +22,9 @@ def initialize(self):
     def execute(self):
         self.subsystem.setRotateAngle(self.angle)
 
-        logging.debug(f"Still moving... {self.subsystem.getAngle()} to {self.angle}")
+        logging.info(
+            f"Shooter still moving... {self.subsystem.getAngle()} to {self.angle}"
+        )
 
     def isFinished(self) -> bool:
         return abs(self.subsystem.getAngle() - self.angle) < 10

rio/constants.py

@@ -198,10 +198,10 @@ class ClimberConstants:
     kServoID = 0
 
     # inversions
-    kInversion = False
+    kInversion = True
 
     # servo angles
-    kServoLock = 0.0
+    kServoLock = -0.1
     kservoOpen = 0.165
 
     # Relationship between climber speed and shooter angle (rough but ugh..)

rio/robotcontainer.py

@@ -171,7 +171,7 @@ def configureButtonBindings(self) -> None:
         # Climbing
         self.opController.rightBumper().whileTrue(
             Climb(
-                self.opController.getRightTriggerAxis()
+                lambda: self.opController.getRightTriggerAxis()
                 - self.opController.getLeftTriggerAxis(),
                 self.climber,
                 self.shooter,