diff --git a/rio/commands/spool.py b/rio/commands/spool.py index 91d6454..0c414c6 100644 --- 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 index bb0d31a..4714b01 100644 --- 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 index 104167b..47a8c89 100644 --- 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 index 2a981a4..be04121 100644 --- 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 index 0240b66..5023176 100644 --- a/rio/subsystems/shooter.py +++ b/rio/subsystems/shooter.py @@ -82,6 +82,8 @@ 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() @@ -89,7 +91,6 @@ def __init__(self) -> None: 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()