From 48855bf656fca33c08bd6520aba379af78b858bc Mon Sep 17 00:00:00 2001 From: Mike Silva Date: Sat, 24 Jan 2026 16:11:54 -0500 Subject: [PATCH] More python fun --- autonomous/__init__.py | 0 components/__init__.py | 7 + components/controller.py | 706 +++++++++++++++++++++++++++++++++++++++ constants.py | 6 + genie.py | 22 ++ pyproject.toml | 2 +- robot.py | 15 +- 7 files changed, 752 insertions(+), 6 deletions(-) create mode 100644 autonomous/__init__.py create mode 100644 components/__init__.py create mode 100644 components/controller.py create mode 100644 genie.py diff --git a/autonomous/__init__.py b/autonomous/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/components/__init__.py b/components/__init__.py new file mode 100644 index 0000000..5d610c7 --- /dev/null +++ b/components/__init__.py @@ -0,0 +1,7 @@ +#from .drivetrain import DriveTrain +from .controller import XboxController + +__all__ = [ + #"DriveTrain", + "XboxController", +] \ No newline at end of file diff --git a/components/controller.py b/components/controller.py new file mode 100644 index 0000000..d600597 --- /dev/null +++ b/components/controller.py @@ -0,0 +1,706 @@ +import wpilib + + +class PlayStationController: + CORRECT_FOR_DEADBAND: bool + DEADBAND: float + PORT: int + VERSION: int = 4 + + def execute(self) -> None: + pass + + def setup(self) -> None: + """ + Setup the PlayStation controller. + This method is called once when the robot is initialized. + """ + if self.VERSION == 5: + self.this_controller = wpilib.PS5Controller(self.PORT) + else: + self.this_controller = wpilib.PS4Controller(self.PORT) + + self.button_was_pressed = { + "Cross": False, + "Circle": False, + "Square": False, + "Triangle": False, + "DPad_Up": False, + "DPad_Down": False, + "DPad_Left": False, + "DPad_Right": False, + "L1": False, + "R1": False, + "L2": False, + "R2": False, + "Options": False, + "Share": False, + } + self.button_is_pressed = { + "Cross": False, + "Circle": False, + "Square": False, + "Triangle": False, + "DPad_Up": False, + "DPad_Down": False, + "DPad_Left": False, + "DPad_Right": False, + "L1": False, + "R1": False, + "L2": False, + "R2": False, + "Options": False, + "Share": False, + } + + def _button_pressed(self, button_name: str, pressed: bool) -> bool: + """ + Generic method to check if a button is pressed. + :param button_name: The name of the button to check. + :param pressed: The current state of the button (True if pressed, False otherwise). + """ + if pressed: + self.button_is_pressed[button_name] = True + else: + if self.button_is_pressed[button_name]: + self.button_was_pressed[button_name] = True + self.button_is_pressed[button_name] = False + return pressed + + def _button_was_pressed(self, button_name: str) -> bool: + """ + Generic method to check if a button was pressed since the last check. + :param button_name: The name of the button to check. + :return: True if the button was pressed since the last check, False otherwise. + """ + if self.button_was_pressed[button_name]: + self.button_was_pressed[button_name] = False + return True + return False + + def _corrected_joystick_value(self, raw_value: float) -> float: + """ + Apply deadband correction to a joystick value. + :param raw_value: The raw joystick value. + :return: The corrected joystick value. + """ + if self.CORRECT_FOR_DEADBAND: + if abs(raw_value) < self.DEADBAND: + return 0.0 + return (raw_value / abs(raw_value)) * ( + (abs(raw_value) - self.DEADBAND) / (1 - self.DEADBAND) + ) + return raw_value + + def capture_button_presses(self) -> None: + """ + Capture the current state of all buttons to track presses. + This method should be called at the end of each control loop iteration. + """ + self.cross_button_pressed() + self.circle_button_pressed() + self.square_button_pressed() + self.triangle_button_pressed() + self.dpad_up_pressed() + self.dpad_down_pressed() + self.dpad_left_pressed() + self.dpad_right_pressed() + self.l1_button_pressed() + self.r1_button_pressed() + self.l2_button_pressed() + self.r2_button_pressed() + self.options_button_pressed() + self.share_button_pressed() + + def get_joysticks(self) -> tuple[float, float, float, float]: + """ + Get the joystick values from the Xbox controller. + + :return: A tuple containing the left joystick x, left joystick y, right joystick x, and right joystick y values. + """ + return ( + self._corrected_joystick_value(self.this_controller.getLeftX()), + self._corrected_joystick_value(self.this_controller.getLeftY()), + self._corrected_joystick_value(self.this_controller.getRightX()), + self._corrected_joystick_value(self.this_controller.getRightY()), + ) + + def cross_button_pressed(self) -> bool: + """ + Check if the Croiss button is pressed. + + :return: True if the Cross button is pressed, False otherwise. + """ + return self._button_pressed("Cross", self.this_controller.getCrossButton()) + + def cross_button_was_pressed(self) -> bool: + """ + Check if the Cross button was pressed since the last check. + + :return: True if the Cross button was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("Cross") + + def circle_button_pressed(self) -> bool: + """ + Check if the Circle button is pressed. + + :return: True if the Circle button is pressed, False otherwise. + """ + return self._button_pressed("Circle", self.this_controller.getCircleButton()) + + def circle_button_was_pressed(self) -> bool: + """ + Check if the Circle button was pressed since the last check. + + :return: True if the Circle button was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("Circle") + + def square_button_pressed(self) -> bool: + """ + Check if the Square button is pressed. + + :return: True if the Square button is pressed, False otherwise. + """ + return self._button_pressed("Square", self.this_controller.getSquareButton()) + + def square_button_was_pressed(self) -> bool: + """ + Check if the Square button was pressed since the last check. + + :return: True if the Square button was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("Square") + + def triangle_button_pressed(self) -> bool: + """ + Check if the Triangle button is pressed. + + :return: True if the Triangle button is pressed, False otherwise. + """ + return self._button_pressed( + "Triangle", self.this_controller.getTriangleButton() + ) + + def triangle_button_was_pressed(self) -> bool: + """ + Check if the Triangle button was pressed since the last check. + + :return: True if the Triangle button was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("Triangle") + + def dpad_up_pressed(self) -> bool: + """ + Check if the D-pad is pressed up. + + :return: True if the D-pad is pressed up, False otherwise. + """ + return self._button_pressed("DPad_Up", self.this_controller.getPOV() == 0) + + def dpad_up_was_pressed(self) -> bool: + """ + Check if the D-pad was pressed up since the last check. + + :return: True if the the D-pad was pressed up since the last check, False otherwise. + """ + return self._button_was_pressed("DPad_Up") + + def dpad_down_pressed(self) -> bool: + """ + Check if the D-pad is pressed down. + + :return: True if the D-pad is pressed down, False otherwise. + """ + return self._button_pressed("DPad_Down", self.this_controller.getPOV() == 180) + + def dpad_down_was_pressed(self) -> bool: + """ + Check if the D-pad was pressed down since the last check. + + :return: True if the the D-pad was pressed down since the last check, False otherwise. + """ + return self._button_was_pressed("DPad_Down") + + def dpad_left_pressed(self) -> bool: + """ + Check if the D-pad is pressed left. + + :return: True if the D-pad is pressed left, False otherwise. + """ + return self._button_pressed("DPad_Left", self.this_controller.getPOV() == 270) + + def dpad_left_was_pressed(self) -> bool: + """ + Check if the D-pad was pressed left since the last check. + + :return: True if the the D-pad was pressed left since the last check, False otherwise. + """ + return self._button_was_pressed("DPad_Left") + + def dpad_right_pressed(self) -> bool: + """ + Check if the D-pad is pressed right. + + :return: True if the D-pad is pressed right, False otherwise. + """ + return self._button_pressed("DPad_Right", self.this_controller.getPOV() == 90) + + def dpad_right_was_pressed(self) -> bool: + """ + Check if the D-pad was pressed right since the last check. + + :return: True if the the D-pad was pressed right since the last check, False otherwise. + """ + return self._button_was_pressed("DPad_Right") + + def r1_button_pressed(self) -> bool: + """ + Check if the R1 button is pressed. + + :return: True if the R1 button is pressed, False otherwise. + """ + return self._button_pressed("R1", self.this_controller.getR1Button()) + + def r1_button_was_pressed(self) -> bool: + """ + Check if the R1 button was pressed since the last check. + + :return: True if the R1 button was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("R1") + + def r2_button_pressed(self) -> bool: + """ + Check if the R2 button is pressed. + + :return: True if the R2 button is pressed, False otherwise. + """ + return self._button_pressed("R2", self.this_controller.getR2Button()) + + def r2_button_was_pressed(self) -> bool: + """ + Check if the R2 button was pressed since the last check. + + :return: True if the R2 button was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("R2") + + def l1_button_pressed(self) -> bool: + """ + Check if the L1 button is pressed. + + :return: True if the L1 button is pressed, False otherwise. + """ + return self._button_pressed("L1", self.this_controller.getL1Button()) + + def l1_button_was_pressed(self) -> bool: + """ + Check if the L1 button was pressed since the last check. + + :return: True if the L1 button was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("L1") + + def l2_button_pressed(self) -> bool: + """ + Check if the L2 button is pressed. + + :return: True if the L2 button is pressed, False otherwise. + """ + return self._button_pressed("L2", self.this_controller.getL2Button()) + + def l2_button_was_pressed(self) -> bool: + """ + Check if the L2 button was pressed since the last check. + + :return: True if the L2 button was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("L2") + + def options_button_pressed(self) -> bool: + """ + Check if the Options button is pressed. + + :return: True if the Options button is pressed, False otherwise. + """ + return self._button_pressed("Options", self.this_controller.getOptionsButton()) + + def options_button_was_pressed(self) -> bool: + """ + Check if the Options button was pressed since the last check. + + :return: True if the Options button was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("Options") + + def share_button_pressed(self) -> bool: + """ + Check if the Share button is pressed. + + :return: True if the Share button is pressed, False otherwise. + """ + if self.VERSION == 4: + return self._button_pressed("Share", self.this_controller.getShareButton()) + else: + return self._button_pressed("Share", False) + + def share_button_was_pressed(self) -> bool: + """ + Check if the Share button was pressed since the last check. + + :return: True if the Share button was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("Share") + + +class XboxController: + CORRECT_FOR_DEADBAND: bool + DEADBAND: float + PORT: int + + def execute(self) -> None: + pass + + def setup(self) -> None: + """ + Setup the Xbox controller. + This method is called once when the robot is initialized. + """ + self.this_controller = wpilib.XboxController(self.PORT) + self.button_was_pressed = { + "A": False, + "B": False, + "X": False, + "Y": False, + "DPad_Up": False, + "DPad_Down": False, + "DPad_Left": False, + "DPad_Right": False, + "Left_Bumper": False, + "Right_Bumper": False, + "Left_Trigger": False, + "Right_Trigger": False, + "Start": False, + "Back": False, + } + self.button_is_pressed = { + "A": False, + "B": False, + "X": False, + "Y": False, + "DPad_Up": False, + "DPad_Down": False, + "DPad_Left": False, + "DPad_Right": False, + "Left_Bumper": False, + "Right_Bumper": False, + "Left_Trigger": False, + "Right_Trigger": False, + "Start": False, + "Back": False, + } + + def _button_pressed(self, button_name: str, pressed: bool) -> bool: + """ + Generic method to check if a button is pressed. + :param button_name: The name of the button to check. + :param pressed: The current state of the button (True if pressed, False otherwise). + """ + if pressed: + self.button_is_pressed[button_name] = True + else: + if self.button_is_pressed[button_name]: + self.button_was_pressed[button_name] = True + self.button_is_pressed[button_name] = False + return pressed + + def _button_was_pressed(self, button_name: str) -> bool: + """ + Generic method to check if a button was pressed since the last check. + :param button_name: The name of the button to check. + :return: True if the button was pressed since the last check, False otherwise. + """ + if self.button_was_pressed[button_name]: + self.button_was_pressed[button_name] = False + return True + return False + + def _corrected_joystick_value(self, raw_value: float) -> float: + """ + Apply deadband correction to a joystick value. + :param raw_value: The raw joystick value. + :return: The corrected joystick value. + """ + if self.CORRECT_FOR_DEADBAND: + if abs(raw_value) < self.DEADBAND: + return 0.0 + return (raw_value / abs(raw_value)) * ( + (abs(raw_value) - self.DEADBAND) / (1 - self.DEADBAND) + ) + return raw_value + + def capture_button_presses(self) -> None: + """ + Capture the current state of all buttons to track presses. + This method should be called at the end of each control loop iteration. + """ + self.a_button_pressed() + self.b_button_pressed() + self.x_button_pressed() + self.y_button_pressed() + self.dpad_up_pressed() + self.dpad_down_pressed() + self.dpad_left_pressed() + self.dpad_right_pressed() + self.left_bumper_pressed() + self.right_bumper_pressed() + self.left_trigger_pressed() + self.right_trigger_pressed() + self.start_button_pressed() + self.back_button_pressed() + + def get_joysticks(self) -> tuple[float, float, float, float]: + """ + Get the joystick values from the Xbox controller. + + :return: A tuple containing the left joystick x, left joystick y, right joystick x, and right joystick y values. + """ + return ( + self._corrected_joystick_value(self.this_controller.getLeftX()), + self._corrected_joystick_value(self.this_controller.getLeftY()), + self._corrected_joystick_value(self.this_controller.getRightX()), + self._corrected_joystick_value(self.this_controller.getRightY()), + ) + + def a_button_pressed(self) -> bool: + """ + Check if the A button is pressed. + + :return: True if the A button is pressed, False otherwise. + """ + return self._button_pressed("A", self.this_controller.getAButton()) + + def a_button_was_pressed(self) -> bool: + """ + Check if the A button was pressed since the last check. + + :return: True if the A button was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("A") + + def b_button_pressed(self) -> bool: + """ + Check if the B button is pressed. + :return: True if the B button is pressed, False otherwise. + """ + return self._button_pressed("B", self.this_controller.getBButton()) + + def b_button_was_pressed(self) -> bool: + """ + Check if the B button was pressed since the last check. + + :return: True if the B button was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("B") + + def x_button_pressed(self) -> bool: + """ + Check if the X button is pressed. + + :return: True if the X button is pressed, False otherwise. + """ + return self._button_pressed("X", self.this_controller.getXButton()) + + def x_button_was_pressed(self) -> bool: + """ + Check if the X button was pressed since the last check. + + :return: True if the X button was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("X") + + def y_button_pressed(self) -> bool: + """ + Check if the Y button is pressed. + + :return: True if the Y button is pressed, False otherwise. + """ + return self._button_pressed("Y", self.this_controller.getYButton()) + + def y_button_was_pressed(self) -> bool: + """ + Check if the Y button was pressed since the last check. + + :return: True if the Y button was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("Y") + + def dpad_up_pressed(self) -> bool: + """ + Check if the D-pad is pressed up. + + :return: True if the D-pad is pressed up, False otherwise. + """ + return self._button_pressed("DPad_Up", self.this_controller.getPOV() == 0) + + def dpad_up_was_pressed(self) -> bool: + """ + Check if the D-pad was pressed up since the last check. + + :return: True if the the D-pad was pressed up since the last check, False otherwise. + """ + return self._button_was_pressed("DPad_Up") + + def dpad_down_pressed(self) -> bool: + """ + Check if the D-pad is pressed down. + + :return: True if the D-pad is pressed down, False otherwise. + """ + return self._button_pressed("DPad_Down", self.this_controller.getPOV() == 180) + + def dpad_down_was_pressed(self) -> bool: + """ + Check if the D-pad was pressed down since the last check. + + :return: True if the the D-pad was pressed down since the last check, False otherwise. + """ + return self._button_was_pressed("DPad_Down") + + def dpad_left_pressed(self) -> bool: + """ + Check if the D-pad is pressed left. + + :return: True if the D-pad is pressed left, False otherwise. + """ + return self._button_pressed("DPad_Left", self.this_controller.getPOV() == 270) + + def dpad_left_was_pressed(self) -> bool: + """ + Check if the D-pad was pressed left since the last check. + + :return: True if the the D-pad was pressed left since the last check, False otherwise. + """ + return self._button_was_pressed("DPad_Left") + + def dpad_right_pressed(self) -> bool: + """ + Check if the D-pad is pressed right. + + :return: True if the D-pad is pressed right, False otherwise. + """ + return self._button_pressed("DPad_Right", self.this_controller.getPOV() == 90) + + def dpad_right_was_pressed(self) -> bool: + """ + Check if the D-pad was pressed right since the last check. + + :return: True if the the D-pad was pressed right since the last check, False otherwise. + """ + return self._button_was_pressed("DPad_Right") + + def left_bumper_pressed(self) -> bool: + """ + Check if the left bumper is pressed. + + :return: True if the left bumper is pressed, False otherwise. + """ + return self._button_pressed("Left_Bumper", self.this_controller.getLeftBumper()) + + def left_bumper_was_pressed(self) -> bool: + """ + Check if the left bumper was pressed since the last check. + + :return: True if the left bumper was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("Left_Bumper") + + def right_bumper_pressed(self) -> bool: + """ + Check if the right bumper is pressed. + + :return: True if the right bumper is pressed, False otherwise. + """ + return self._button_pressed( + "Right_Bumper", self.this_controller.getRightBumper() + ) + + def right_bumper_was_pressed(self) -> bool: + """ + Check if the right bumper was pressed since the last check. + + :return: True if the right bumper was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("Right_Bumper") + + def left_trigger_pressed(self) -> float: + """ + Get the value of the left trigger. + + :return: The value of the left trigger, ranging from 0.0 to 1.0. + """ + self._button_pressed( + "Left_Trigger", self.this_controller.getLeftTriggerAxis() >= 0.5 + ) + return self.this_controller.getLeftTriggerAxis() + + def left_trigger_was_pressed(self) -> bool: + """ + Check if the left trigger was pressed since the last check. + + :return: True if the left trigger was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("Left_Trigger") + + def right_trigger_pressed(self) -> float: + """ + Get the value of the right trigger. + + :return: The value of the right trigger, ranging from 0.0 to 1.0. + """ + self._button_pressed( + "Right_Trigger", self.this_controller.getRightTriggerAxis() >= 0.5 + ) + return self.this_controller.getRightTriggerAxis() + + def right_trigger_was_pressed(self) -> bool: + """ + Check if the right trigger was pressed since the last check. + + :return: True if the right trigger was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("Right_Trigger") + + def start_button_pressed(self) -> bool: + """ + Check if the start button is pressed. + + :return: True if the start button is pressed, False otherwise. + """ + return self._button_pressed("Start", self.this_controller.getStartButton()) + + def start_button_was_pressed(self) -> bool: + """ + Check if the start button was pressed since the last check. + + :return: True if the start button was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("Start") + + def back_button_pressed(self) -> bool: + """ + Check if the back button is pressed. + + :return: True if the back button is pressed, False otherwise. + """ + return self._button_pressed("Back", self.this_controller.getBackButton()) + + def back_button_was_pressed(self) -> bool: + """ + Check if the back button was pressed since the last check. + + :return: True if the back button was pressed since the last check, False otherwise. + """ + return self._button_was_pressed("Back") diff --git a/constants.py b/constants.py index e69de29..19b80bd 100644 --- a/constants.py +++ b/constants.py @@ -0,0 +1,6 @@ +# Should we apply deadband correction to the controller inputs? +CONTROLLER_CORRECT_FOR_DEADBAND = True +# The deadband for the controller joysticks +CONTROLLER_DEADBAND = 0.3 +# The port the controller is connected to +CONTROLLER_PORT = 0 \ No newline at end of file diff --git a/genie.py b/genie.py new file mode 100644 index 0000000..96ecd4e --- /dev/null +++ b/genie.py @@ -0,0 +1,22 @@ +import commands2 +import magicbot + + +class GenieRobot(magicbot.MagicRobot): + def robotPeriodic(self) -> None: + commands2.CommandScheduler.getInstance().run() + return super().robotPeriodic() + + def scheduleCommand(self, command: commands2.Command) -> None: + """ + Schedule a command to run. This is a wrapper around + :meth:`CommandScheduler.add` that allows you to + schedule commands. + + :param command: The command to schedule + """ + inst = commands2.CommandScheduler.getInstance() + inst.schedule(command) + + def disabledPeriodic(self): + pass \ No newline at end of file diff --git a/pyproject.toml b/pyproject.toml index b8f2c3e..5da83eb 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -13,7 +13,7 @@ robotpy_version = "2026.2.1" components = [ #"all", "apriltag", - # "commands2", + "commands2", # "cscore", # "romi", # "sim", diff --git a/robot.py b/robot.py index 29b5586..32e6331 100644 --- a/robot.py +++ b/robot.py @@ -1,15 +1,20 @@ -import magicbot -import wpilib +import components +import constants +import genie -class MyRobot(magicbot.MagicRobot): +class MyRobot(genie.GenieRobot): + CONTROLLER: components.XboxController def createObjects(self): '''Create motors and stuff here''' - pass + # Controller stuff here + self.CONTROLLER_CORRECT_FOR_DEADBAND = constants.CONTROLLER_CORRECT_FOR_DEADBAND + self.CONTROLLER_DEADBAND = constants.CONTROLLER_DEADBAND + self.CONTROLLER_PORT = constants.CONTROLLER_PORT def teleopInit(self): '''Called when teleop starts; optional''' def teleopPeriodic(self): '''Called periodically during teleop''' - pass \ No newline at end of file + self.CONTROLLER.capture_button_presses() \ No newline at end of file