From 9c95706a37c18ee4100d89f5c3ac2c17388bc9c5 Mon Sep 17 00:00:00 2001
From: KenwoodFox <kenwood364@gmail.com>
Date: Thu, 29 Feb 2024 18:07:00 -0500
Subject: [PATCH] Destroy swerve module

---
 rio/commands/climb.py      | 26 ++++++++++++++++++++++----
 rio/commands/shooterROT.py |  4 +++-
 rio/constants.py           |  4 ++--
 rio/robotcontainer.py      |  2 +-
 4 files changed, 28 insertions(+), 8 deletions(-)

diff --git a/rio/commands/climb.py b/rio/commands/climb.py
index ce092c6..22e919f 100644
--- a/rio/commands/climb.py
+++ b/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)
diff --git a/rio/commands/shooterROT.py b/rio/commands/shooterROT.py
index b515f5f..9d35a64 100644
--- a/rio/commands/shooterROT.py
+++ b/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
diff --git a/rio/constants.py b/rio/constants.py
index 9d048c0..4b1803e 100644
--- a/rio/constants.py
+++ b/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..)
diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py
index 380b812..ae90b24 100644
--- a/rio/robotcontainer.py
+++ b/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,