janky but I don't want to be a robotoics all night and this will work…

… better than nothing

rio/commands/climb.py

@@ -1,6 +1,5 @@
-import typing
-import logging
 import commands2
+import rev
 
 from constants import ClimberConstants
 
@@ -11,10 +10,7 @@
 class Climb(commands2.Command):
     def __init__(
         self,
-        _speed: typing.Callable[
-            [],
-            float,
-        ],
+        _speed,
         _climber: Climber,
         _shooter: Shooter,
     ):
@@ -33,23 +29,12 @@ def __init__(
         # Command requires
         self.addRequirements((self.climber, self.shooter))
 
+        self.shooter.setIdleMode(rev._rev.CANSparkBase.IdleMode.kCoast)
+        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.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.shooter.setIdleMode(rev._rev.CANSparkBase.IdleMode.kBrake)
         self.climber.setClimberMotorSpeed(0)

rio/commands/resetYaw.py

@@ -0,0 +1,22 @@
+import commands2
+
+from subsystems.drivesubsystem import DriveSubsystem
+
+
+class ResetYaw(commands2.Command):
+    def __init__(
+        self,
+        _drivetrain=DriveSubsystem,
+        angle=0,
+    ):
+        super().__init__()
+
+        # local subsystem instance
+        self.subsystem = _drivetrain
+        self.angle = angle
+
+    def initialize(self):
+        self.subsystem.resetYaw(self.angle)
+
+    def end(self, interrupted: bool):
+        return True

rio/commands/spool.py

@@ -0,0 +1,35 @@
+import commands2
+
+from constants import ClimberConstants
+
+from subsystems.climber import Climber
+
+
+class Spool(commands2.Command):
+    def __init__(
+        self,
+        _speed,
+        _climber: Climber,
+    ):
+        """
+        rotates the climber
+        """
+        super().__init__()
+
+        # local instance of subsystem
+        self.climber = _climber
+
+        # local var of speed
+        self.speed = _speed
+
+        # Command requires
+        self.addRequirements((self.climber))
+
+    def initialize(self):
+        self.climber.setServoAngle(ClimberConstants.kservoOpen)
+
+    def execute(self):
+        self.climber.setClimberMotorSpeed(self.speed)
+
+    def end(self, interrupted: bool):
+        self.climber.setClimberMotorSpeed(0)

rio/pyproject.toml

@@ -6,7 +6,7 @@
 [tool.robotpy]
 
 # Version of robotpy this project depends on
-robotpy_version = "2024.2.1.1"
+robotpy_version = "2024.3.1.0"
 
 # Which extra RobotPy components should be installed
 # -> equivalent to `pip install robotpy[extra1, ...]
@@ -27,4 +27,4 @@ robotpy_extras = [
 ]
 
 # Other pip packages to install
-requires = []
+requires = []

rio/robotcontainer.py

@@ -16,7 +16,7 @@
 # CommandsV2
 import commands2
 from commands2 import cmd
-from commands2.button import CommandJoystick, CommandXboxController
+from commands2.button import CommandXboxController, CommandGenericHID
 
 from constants import AutoConstants, DriveConstants, OIConstants, SuperStrucConstants
 
@@ -38,7 +38,8 @@
 from commands.setIntakeSpeed import SetIntakeSpeed
 from commands.loadMagazine import LoadMagazine
 from commands.climb import Climb
-from commands.lock import Lock
+from commands.resetYaw import ResetYaw
+from commands.spool import Spool
 
 # NetworkTables
 from ntcore import NetworkTableInstance
@@ -94,8 +95,8 @@ def __init__(self) -> None:
                         self.driverController.getRawAxis(4),
                         OIConstants.kDriveDeadband,  # TODO: Use constants to set these controls
                     )
-                    * 0.6,
-                    lambda: self.fieldCentricChooser.getSelected(),
+                    * 0.5,
+                    False,
                     True,
                 ),
                 self.robotDrive,
@@ -165,6 +166,11 @@ def configureButtonBindings(self) -> None:
                 FlyWheelSpeed(0, self.shooter),  # stops the Flywheel
             )
         )
+        self.driverController.start().onTrue(ResetYaw(self.robotDrive))
+
+        self.driverController.back().onTrue(
+            commands2.InstantCommand(self.intake.zeroIntake)
+        )
 
         # Climbing
         self.driverController.rightBumper().whileTrue(
@@ -180,33 +186,26 @@ def configureButtonBindings(self) -> None:
         #        Operator Commands
         # ==============================
 
-        # Rotating intake but Manually
-        self.opController.pov(90).whileTrue(IntakeRotationMAN(1, self.intake))  # out
-        self.opController.pov(270).whileTrue(IntakeRotationMAN(-1, self.intake))  # in
+        # intake keybinds
+        # intake movement
+        self.opController.button(2).whileTrue(IntakeRotationMAN(1, self.intake))  # out
+        self.opController.button(1).whileTrue(IntakeRotationMAN(-1, self.intake))  # in
 
-        # Rotating shooter but Manually
-        self.opController.pov(0).whileTrue(manualROT(0.5, self.shooter))  # out
-        self.opController.pov(180).whileTrue(manualROT(-0.5, self.shooter))  # in
+        # intake spin
+        self.opController.button(6).whileTrue(SetIntakeSpeed(0.6, self.intake))
+        self.opController.button(9).whileTrue(SetIntakeSpeed(-0.6, self.intake))
 
-        # Spinning intake wheels but Manually
-        self.opController.a().whileTrue(SetIntakeSpeed(0.5, self.intake))  # suck
-        self.opController.b().whileTrue(SetIntakeSpeed(-0.5, self.intake))  # blow
+        # shooter keybinds
+        # shooter movement
+        self.opController.button(3).whileTrue(manualROT(0.5, self.shooter))
+        self.opController.button(4).whileTrue(manualROT(-0.5, self.shooter))
 
-        # Spinning flywheel but Manually
-        self.opController.rightBumper().whileTrue(
-            FlyWheelSpeed(0.5, self.intake)
-        )  # shoot
-        self.opController.leftBumper().whileTrue(
-            FlyWheelSpeed(-0.5, self.intake)
-        )  # reject
+        # climber
+        self.opController.button(5).whileTrue(Climb(0.2, self.climber, self.shooter))
+        self.opController.button(7).whileTrue(Spool(-0.1, self.climber))
 
-        # Cancel all when x is pressed
-        self.opController.x().onTrue(commands2.InstantCommand(self.cancelAll))
-
-        # Zeroes the intake
-        self.opController.back().onTrue(
-            commands2.InstantCommand(self.intake.zeroIntake)
-        )
+        # Cancel all
+        self.opController.button(8).onTrue(commands2.InstantCommand(self.cancelAll))
 
     def cancelAll(self) -> None:
         """

rio/simgui-window.json

@@ -18,7 +18,7 @@
     "###FMS": {
       "Collapsed": "0",
       "Pos": "5,540",
-      "Size": "169,184"
+      "Size": "283,140"
     },
     "###Joysticks": {
       "Collapsed": "0",

rio/subsystems/drivesubsystem.py

@@ -341,3 +341,6 @@ def getTurnRate(self) -> float:
         :returns: The turn rate of the robot, in degrees per second
         """
         return self.gyro.getRate() * (-1.0 if DriveConstants.kGyroReversed else 1.0)
+
+    def resetYaw(self, angle) -> None:
+        self.gyro.setYaw(angle, 100)

rio/subsystems/intake.py

@@ -77,7 +77,6 @@ def periodic(self) -> None:
         self.sd.putNumber("Thermals/Intake", self.intakeMotor.getMotorTemperature())
         self.sd.putNumber("Intake/IntakeAngle", self.intakeEncoder.getPosition())
         self.sd.putNumber("Thermals/Lift", self.liftMotor.getMotorTemperature())
-        # print(self.liftEncoder.getPosition())
 
     def intake(self, speed):
         self.intakeMotor.set(speed)

rio/subsystems/shooter.py

@@ -1,4 +1,6 @@
 import wpilib
+import rev
+
 from ntcore import NetworkTableInstance
 from commands2 import Subsystem
 from rev import (
@@ -78,16 +80,15 @@ def __init__(self) -> None:
         # (joe added this, its bad)
         self.flyPIDController.setOutputRange(-1, 1)
 
-        # curr limit so the motor doesn't die
-        self.rotateMotor.setSmartCurrentLimit(8)
+        self.rotateMotor.setIdleMode(rev._rev.CANSparkBase.IdleMode.kBrake)
 
         # 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)
@@ -134,3 +135,8 @@ def getVelocity(self) -> float:
 
     def setFlyVelocity(self, velocity):
         self.flyPIDController.setReference(velocity, CANSparkMax.ControlType.kVelocity)
+
+    # hello burflash my old friend
+    def setIdleMode(self, mode):
+        self.rotateMotor.setIdleMode(mode)
+        self.rotateMotor.burnFlash()