Merge pull request #2 from AlphaKnights/command-based

convert all drive into command based

commands/arcadedrive.py

@@ -0,0 +1,20 @@
+import commands2
+import typing
+from subsystems.drivetrain import arcadeDrive
+
+class ArcadeDrive(commands2.CommandBase):
+    def __init__(self, arcade_drive: arcadeDrive, y: typing.Callable[[], float], z: typing.Callable[[], float]) -> None:
+        super().__init__()
+        self.arcade_drive = arcade_drive
+        self.y = y
+        self.z = z
+        self.addRequirements(arcade_drive)
+
+    def execute(self) -> None:
+        self.arcade_drive.arcadeDrive(self.y(), self.z())
+
+    def isFinished(self) -> bool:
+        return False
+
+    def end(self) -> None:
+        self.arcade_drive.arcadeDrive(0, 0)

commands/limelightdisplay.py

@@ -0,0 +1,20 @@
+import commands2
+import typing
+from subsystems.limelight import limelightSystem
+
+class limelightDisplay(commands2.CommandBase):
+    def __init__(self, limelight: limelightSystem) -> None:
+        super().__init__()
+        self.limelight = limelight
+        self.addRequirements(limelight)
+
+    def execute(self) -> None:
+        # print(self.limelight.get_results().tagId)
+        results = self.limelight.get_results()
+        print(results.tagId if results else 'none')
+
+    def isFinished(self) -> bool:
+        return False
+
+    def end(self) -> None:
+        pass

constants.py

@@ -7,7 +7,7 @@
 
 PITCH_MOTOR_ID = 42
 
-DRIVE_JOYSTICK_PORT = 0
+DRIVE_JOYSTICK_PORT = 1
 
 
 FIRE_UPPER_BUTTON_ID = 1

interfaces/limelight_results.py

@@ -0,0 +1,5 @@
+class limelightResults:
+    tagId: int
+
+    def __init__(self, data: dict) -> None:
+        self.tagId = data["Fiducial"][0]["fID"]

pyproject.toml

@@ -13,7 +13,7 @@ robotpy_version = "2024.3.2.2"
 robotpy_extras = [
     # "all",
     # "apriltag",
-    # "commands2",
+     "commands2",
     # "cscore",
     # "navx",
     # "pathplannerlib",
@@ -32,6 +32,5 @@ requires = [
     "pylint>=3.2.6",
     "mypy>=1.11.0",
     "robotpy-ctre>=2024.1.3",
-    "limelightlib-python>=0.9.6",   
-
+    "limelightlib-python>=0.9.6",
 ]

robot.py

@@ -12,86 +12,23 @@
 import constants
 import math
 import limelight 
+import commands2
+import typing
 
-class MyRobot(wpilib.TimedRobot):
+from robotcontainer import RobotContainer
+
+class MyRobot(commands2.TimedCommandRobot):
     """
     This is a demo program showing the use of the DifferentialDrive class.
     Runs the motors with arcade steering.
     """  
-    def robotInit(self):
-        """Robot initialization function"""
-        self.pitch_motor = phoenix5.WPI_TalonFX(constants.PITCH_MOTOR_ID)
-        self.pov_up_check = False
-
-        left_motor_1 = phoenix5.WPI_TalonSRX(constants.LEFT_MOTOR_1_ID)
-        left_motor_2 = phoenix5.WPI_TalonSRX(constants.LEFT_MOTOR_2_ID)
-        right_motor_1 = phoenix5.WPI_TalonSRX(constants.RIGHT_MOTOR_1_ID)
-        right_motor_2 = phoenix5.WPI_TalonSRX(constants.RIGHT_MOTOR_2_ID)
-        left_motor_1.follow(left_motor_2)
-        right_motor_1.follow(right_motor_2)
-        self.robot_drive = wpilib.drive.DifferentialDrive(left_motor_1, right_motor_1)
-        self.stick = wpilib.Joystick(constants.DRIVE_JOYSTICK_PORT)
-        pneumatic_hub = wpilib.PneumaticHub(constants.PNEUMATIC_HUB_PORT)
-        self.upper_solenoid_1 = pneumatic_hub.makeSolenoid(constants.UPPER_SOLENOID_1_CHANNEL)
-        self.upper_solenoid_2 = pneumatic_hub.makeSolenoid(constants.UPPER_SOLENOID_2_CHANNEL)
-        self.upper_solenoid_3 = pneumatic_hub.makeSolenoid(constants.UPPER_SOLENOID_3_CHANNEL)
-        self.lower_solenoid_1 = pneumatic_hub.makeSolenoid(constants.LOWER_SOLENOID_1_CHANNEL)
-        self.lower_solenoid_2 = pneumatic_hub.makeSolenoid(constants.LOWER_SOLENOID_2_CHANNEL)
-        self.lower_solenoid_3 = pneumatic_hub.makeSolenoid(constants.LOWER_SOLENOID_3_CHANNEL)
-
-        self.upper_solenoid_1.setPulseDuration(constants.SOLENOID_PULSE_LENGTH)
-        self.upper_solenoid_2.setPulseDuration(constants.SOLENOID_PULSE_LENGTH)
-        self.upper_solenoid_3.setPulseDuration(constants.SOLENOID_PULSE_LENGTH)
-        self.lower_solenoid_1.setPulseDuration(constants.SOLENOID_PULSE_LENGTH)
-        self.lower_solenoid_2.setPulseDuration(constants.SOLENOID_PULSE_LENGTH)
-        self.lower_solenoid_3.setPulseDuration(constants.SOLENOID_PULSE_LENGTH)
-        self.discovered_limelights = limelight.discover_limelights(debug=True)
-        print("discovered limelights:", self.discovered_limelights)
-
-
-        # We need to invert one side of the drivetrain so that positive voltages
-        # result in both sides moving forward. Depending on how your robot's
-        # gearbox is constructed, you might have to invert the left side instead.
-        right_motor_1.setInverted(True)
 
-    # def RobotPeriodic(self):
-    #     # Drive with arcade drive.
-    #     # That means that the Y axis drives forward
-    #     # and backward, and the X turns left and right.
-    #     self.robot_drive.arcadeDrive(self.stick.getY(), self.stick.getX())
-        # self.left_motor_1.set(self.stick.getY())
-
-    def fire_upper(self) :
-        self.upper_solenoid_1.startPulse()
-        self.upper_solenoid_2.startPulse()
-        self.upper_solenoid_3.startPulse()
-
-    def fire_lower(self) :
-        self.lower_solenoid_1.startPulse()
-        self.lower_solenoid_2.startPulse()
-        self.lower_solenoid_3.startPulse()
+    autonomousCommand: typing.Optional[commands2.Command] = None
 
+    def robotInit(self):
+        """Robot initialization function"""
 
+        self.container = RobotContainer()
     def teleopPeriodic(self):
-        # Drive with arcade drive.
-        # That means that the Y axis drives forward
-        # and backward, and the X turns left and right.
-        self.robot_drive.arcadeDrive(self.stick.getY(), self.stick.getZ())
-        if self.stick.getPOV(0) != -1 :
-            self.pitch_motor.set(math.cos(self.stick.getPOV(0) * math.pi / 180))
-        else :
-            self.pitch_motor.set(0)
-        if self.discovered_limelights:
-            limelight_address = self.discovered_limelights[0] 
-            ll = limelight.Limelight(limelight_address)
-            results = ll.get_results()
-            status = ll.get_status()
-            if results["botpose_tagcount"] != 0:
-                print("targeting results:", results["Fiducial"][0]["fID"])
-
-        if self.stick.getRawButton(constants.FIRE_UPPER_BUTTON_ID) :
-            self.fire_upper()
-        if self.stick.getRawButton(constants.FIRE_LOWER_BUTTON_ID) :
-            self.fire_lower()
-
+        commands2.CommandScheduler.getInstance().run()
 

robotcontainer.py

@@ -0,0 +1,22 @@
+import constants
+
+import commands2
+import commands2.button
+
+import wpilib
+
+# from subsystems.drivetrain import arcadeDrive
+# from commands.arcadedrive import ArcadeDrive
+
+from subsystems.limelight import limelightSystem
+from commands.limelightdisplay import limelightDisplay
+
+class RobotContainer:
+    def __init__(self) -> None:
+        self.limelight = limelightSystem()
+        self.stick = wpilib.Joystick(constants.DRIVE_JOYSTICK_PORT)
+        self.configureButtonBindings()
+
+    def configureButtonBindings(self) -> None:
+        # self.drivetrain.setDefaultCommand(ArcadeDrive(self.drivetrain, lambda: self.stick.getX(), lambda: self.stick.getY()))
+        self.limelight.setDefaultCommand(limelightDisplay(self.limelight))

subsystems/drivetrain.py

@@ -0,0 +1,19 @@
+import commands2
+import phoenix5
+import constants
+import wpilib
+import wpilib.drive
+
+class arcadeDrive(commands2.Subsystem):
+    def __init__(self) -> None:
+        super().__init__()
+        self.left_motor_1 = phoenix5.WPI_TalonSRX(constants.LEFT_MOTOR_1_ID)
+        self.left_motor_2 = phoenix5.WPI_TalonSRX(constants.LEFT_MOTOR_2_ID)
+        self.right_motor_1 = phoenix5.WPI_TalonSRX(constants.RIGHT_MOTOR_1_ID)
+        self.right_motor_2 = phoenix5.WPI_TalonSRX(constants.RIGHT_MOTOR_2_ID)
+        self.left_motor_1.follow(self.left_motor_2)
+        self.right_motor_1.follow(self.right_motor_2)
+        self.robot_drive = wpilib.drive.DifferentialDrive(self.left_motor_1, self.right_motor_1)
+
+    def arcadeDrive(self, y: float, z: float) -> None:
+        self.robot_drive.arcadeDrive(y, z)

subsystems/limelight.py

@@ -0,0 +1,25 @@
+import typing
+import commands2
+import constants
+import wpilib
+import limelight
+from interfaces.limelight_results import limelightResults
+
+class limelightSystem(commands2.Subsystem):
+    def __init__(self) -> None:
+        super().__init__()
+
+        limelights = limelight.discover_limelights(debug=True)
+
+        if not limelights:
+            raise ValueError("No limelights found")
+
+        self.limelight = limelight.Limelight(limelights[0])
+
+    def get_results(self, debug=False) -> typing.Optional[limelightResults]:
+        results = self.limelight.get_results()
+
+        if results["botpose_tagcount"] == 0:
+            return
+
+        return limelightResults(results)