Climber Rework (#23)

Co-authored-by: kredcool <[email protected]>
FRC-1721 · Mar 13, 2024 · 0d748ea · 0d748ea
1 parent c7d1934
commit 0d748ea
Show file tree

Hide file tree

Showing 5 changed files with 43 additions and 11 deletions.
diff --git a/rio/commands/spool.py b/rio/commands/spool.py
@@ -1,15 +1,22 @@
+import typing
+import logging
 import commands2
 
 from constants import ClimberConstants
 
 from subsystems.climber import Climber
+from subsystems.shooter import Shooter
 
 
 class Spool(commands2.Command):
     def __init__(
         self,
-        _speed,
+        _speed: typing.Callable[
+            [],
+            float,
+        ],
         _climber: Climber,
+        _shooter: Shooter,
     ):
         """
         rotates the climber
@@ -18,18 +25,38 @@ def __init__(
 
         # local instance of subsystem
         self.climber = _climber
+        self.shooter = _shooter
 
         # local var of speed
         self.speed = _speed
 
         # Command requires
-        self.addRequirements((self.climber))
+        self.addRequirements((self.climber, self.shooter))
 
-    def initialize(self):
-        self.climber.setServoAngle(ClimberConstants.kservoOpen)
+        self.climber.setServoAngle(ClimberConstants.kServoLock)
 
     def execute(self):
+        logging.debug(f"Climb is {self.speed}")
+
+        if self.speed == 0 or self.speed >= 0.5:
+            # Positive numbers imply we're pulling DOWN
+            self.shooter.currlimit(1)
+            self.climber.setServoAngle(ClimberConstants.kServoLock)
+            self.shooter.rotateManual(
+                -self.speed * ClimberConstants.kClimberShooterForward
+            )
+        else:
+            # Negative numbers imply we're relaxing
+            self.shooter.currlimit(2)
+            self.climber.setServoAngle(ClimberConstants.kservoOpen)
+            self.shooter.rotateManual(
+                -self.speed * ClimberConstants.kClimberShooterBackward
+            )
+
         self.climber.setClimberMotorSpeed(self.speed)
 
     def end(self, interrupted: bool):
+        self.climber.setServoAngle(ClimberConstants.kServoLock)
+        self.shooter.currlimit(35)
         self.climber.setClimberMotorSpeed(0)
+        self.shooter.rotateManual(0)
diff --git a/rio/constants.py b/rio/constants.py
@@ -213,5 +213,5 @@ class ClimberConstants:
     kservoOpen = 0.165
 
     # Relationship between climber speed and shooter angle (rough but ugh..)
-    kClimberShooterForward = 0.3  # Down
-    kClimberShooterBackward = 0.5  # Up
+    kClimberShooterForward = 0.4  # Down
+    kClimberShooterBackward = 0.75  # Up
diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py
@@ -202,8 +202,8 @@ def configureButtonBindings(self) -> None:
         self.opController.button(4).whileTrue(manualROT(-0.5, self.shooter))
 
         # climber
-        self.opController.button(5).whileTrue(Climb(0.2, self.climber, self.shooter))
-        self.opController.button(7).whileTrue(Spool(-0.1, self.climber))
+        self.opController.button(5).whileTrue(Spool(0.2, self.climber, self.shooter))
+        self.opController.button(7).whileTrue(Spool(-0.1, self.climber, self.shooter))
 
         # Cancel all
         self.opController.button(8).onTrue(commands2.InstantCommand(self.cancelAll))

diff --git a/rio/simgui-window.json b/rio/simgui-window.json
@@ -6,7 +6,7 @@
     "GLOBAL": {
       "fps": "120",
       "height": "699",
-      "maximized": "1",
+      "maximized": "0",
       "style": "0",
       "userScale": "2",
       "width": "1366",
@@ -18,7 +18,7 @@
     "###FMS": {
       "Collapsed": "0",
       "Pos": "5,540",
-      "Size": "283,140"
+      "Size": "169,184"
     },
     "###Joysticks": {
       "Collapsed": "0",

diff --git a/rio/subsystems/shooter.py b/rio/subsystems/shooter.py
@@ -82,14 +82,15 @@ def __init__(self) -> None:
 
         self.rotateMotor.setIdleMode(rev._rev.CANSparkBase.IdleMode.kBrake)
 
+        self.rotateMotor.setSmartCurrentLimit(35)
+
         # Burn flymotor configuration
         self.flyMotor.burnFlash()
         self.rotateMotor.burnFlash()
 
     def periodic(self) -> None:
         self.sd.putNumber("Thermals/rotate", self.rotateMotor.getMotorTemperature())
         self.sd.putNumber("Thermals/fly", self.flyMotor.getMotorTemperature())
-        print(self.rotateEncoder.getPosition())
 
     def setFlyWheelSpeed(self, speed):
         self.flyMotor.set(speed)
@@ -141,3 +142,7 @@ def setFlyVelocity(self, velocity):
     def setIdleMode(self, mode):
         self.rotateMotor.setIdleMode(mode)
         self.rotateMotor.burnFlash()
+
+    def currlimit(self, currlmt):
+        self.rotateMotor.setSmartCurrentLimit(currlmt)
+        self.rotateMotor.burnFlash()