Feat/controllers

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/setIntakeSpeed.py

@@ -29,7 +29,7 @@ def execute(self):
 
     def isFinished(self):
         return True
-    
+
     def end(self, interrupted: bool):
         logging.debug(f"Intake suck done")
         return True

rio/constants.py

@@ -147,7 +147,6 @@ class AutoConstants:
 
 
 class SuperStrucConstants:
-
     # angles for shooter
     ShootPos = 324
     LoadPos = 208

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
 
@@ -35,6 +35,7 @@
 from commands.intakeUntilNote import intakeUntilNote
 from commands.setIntakeSpeed import SetIntakeSpeed
 from commands.loadMagazine import LoadMagazine
+from commands.resetYaw import ResetYaw
 
 # NetworkTables
 from ntcore import NetworkTableInstance
@@ -90,13 +91,22 @@ def __init__(self) -> None:
                         OIConstants.kDriveDeadband,  # TODO: Use constants to set these controls
                     )
                     * 0.6,
-                    False,
+                    True,
                     True,
                 ),
                 self.robotDrive,
             )
         )
 
+        self.shooter.setDefaultCommand(
+            commands2.cmd.run(
+                lambda: FlyWheelSpeed(
+                    self.opController.getRawAxis(2) - 0.16, self.shooter
+                ),
+                self.shooter,
+            )
+        )
+
     def configureButtonBindings(self) -> None:
         """
         Use this method to define your button->command mappings. Buttons can be created by
@@ -160,36 +170,41 @@ 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)
+        )
 
         # ==============================
         #        Operator Commands
         # ==============================
 
-        # moving intake
-        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
 
-        # _____POST_INTAKE_KEYBINDS_____
+        # intake spin
+        self.opController.button(6).whileTrue(SetIntakeSpeed(0.6, self.intake))
+        self.opController.button(9).whileTrue(SetIntakeSpeed(-0.6, self.intake))
 
-        # moving shooter
-        self.opController.pov(0).whileTrue(manualROT(0.5, self.shooter))
-        self.opController.pov(180).whileTrue(manualROT(-0.5, self.shooter))
+        # shooter keybinds
+        # shooter movement
+        self.opController.button(3).whileTrue(manualROT(0.5, self.shooter))
+        self.opController.button(4).whileTrue(manualROT(-0.5, self.shooter))
 
-        self.driverController.pov(0).whileTrue(ShooterROT(60, self.shooter))
-
-        # PID shooter rotation (NOT CURRENTLY WORKING)
-
-        # self.opController.pov(0).whileTrue(ShooterROT(0,self.shooter))  # out
-        # self.opController.pov(180).whileTrue(ShooterROT(40,self.shooter))  # out
-
-        self.opController.b().whileTrue(RotateIntake(60, self.intake))
-
-        # Cancel all when x is pressed
-        self.opController.x().onTrue(commands2.InstantCommand(self.cancelAll))
+        """
+        # climber
+        if (
+            self.opController.getRawAxis(0) > 0.1
+            or self.opController.getRawAxis(0) > -0.1
+        ):
+            climb(self.opController.getRawAxis(0),self.climber)
+        """
 
-        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/subsystems/drivesubsystem.py

@@ -301,3 +301,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)