diff --git a/commands/arcadedrive.py b/commands/arcadedrive.py new file mode 100644 index 0000000..17b60bc --- /dev/null +++ b/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) \ No newline at end of file diff --git a/commands/limelightdisplay.py b/commands/limelightdisplay.py new file mode 100644 index 0000000..9d80224 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/constants.py b/constants.py index 59a2db2..a610755 100644 --- a/constants.py +++ b/constants.py @@ -7,7 +7,7 @@ PITCH_MOTOR_ID = 42 -DRIVE_JOYSTICK_PORT = 0 +DRIVE_JOYSTICK_PORT = 1 FIRE_UPPER_BUTTON_ID = 1 diff --git a/interfaces/limelight_results.py b/interfaces/limelight_results.py new file mode 100644 index 0000000..31fd141 --- /dev/null +++ b/interfaces/limelight_results.py @@ -0,0 +1,5 @@ +class limelightResults: + tagId: int + + def __init__(self, data: dict) -> None: + self.tagId = data["Fiducial"][0]["fID"] \ No newline at end of file diff --git a/pyproject.toml b/pyproject.toml index 4b2ad92..ece0364 100644 --- a/pyproject.toml +++ b/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", ] diff --git a/robot.py b/robot.py index 9e63dad..7376a44 100644 --- a/robot.py +++ b/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() \ No newline at end of file diff --git a/robotcontainer.py b/robotcontainer.py new file mode 100644 index 0000000..61942ea --- /dev/null +++ b/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)) \ No newline at end of file diff --git a/subsystems/drivetrain.py b/subsystems/drivetrain.py new file mode 100644 index 0000000..149a1fe --- /dev/null +++ b/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) \ No newline at end of file diff --git a/subsystems/limelight.py b/subsystems/limelight.py new file mode 100644 index 0000000..8ec8bad --- /dev/null +++ b/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)