From 2a8ef35880f2a658136459d99eb3584c642e45ea Mon Sep 17 00:00:00 2001 From: CoryNessCTR Date: Fri, 8 Dec 2023 13:00:42 -0500 Subject: [PATCH 01/16] Adds Python examples --- python/StatusSignals/.deploy_cfg | 3 ++ python/StatusSignals/.gitignore | 2 ++ python/StatusSignals/.installer_config | 3 ++ python/StatusSignals/robot.py | 49 ++++++++++++++++++++++++++ python/TankDrive/.deploy_cfg | 3 ++ python/TankDrive/.gitignore | 2 ++ python/TankDrive/.installer_config | 3 ++ python/TankDrive/robot.py | 49 ++++++++++++++++++++++++++ 8 files changed, 114 insertions(+) create mode 100644 python/StatusSignals/.deploy_cfg create mode 100644 python/StatusSignals/.gitignore create mode 100644 python/StatusSignals/.installer_config create mode 100644 python/StatusSignals/robot.py create mode 100644 python/TankDrive/.deploy_cfg create mode 100644 python/TankDrive/.gitignore create mode 100644 python/TankDrive/.installer_config create mode 100644 python/TankDrive/robot.py diff --git a/python/StatusSignals/.deploy_cfg b/python/StatusSignals/.deploy_cfg new file mode 100644 index 00000000..e6134433 --- /dev/null +++ b/python/StatusSignals/.deploy_cfg @@ -0,0 +1,3 @@ +[auth] +hostname = 7762 + diff --git a/python/StatusSignals/.gitignore b/python/StatusSignals/.gitignore new file mode 100644 index 00000000..a1d0392d --- /dev/null +++ b/python/StatusSignals/.gitignore @@ -0,0 +1,2 @@ +ctre_sim +tests diff --git a/python/StatusSignals/.installer_config b/python/StatusSignals/.installer_config new file mode 100644 index 00000000..e6134433 --- /dev/null +++ b/python/StatusSignals/.installer_config @@ -0,0 +1,3 @@ +[auth] +hostname = 7762 + diff --git a/python/StatusSignals/robot.py b/python/StatusSignals/robot.py new file mode 100644 index 00000000..95ecc35a --- /dev/null +++ b/python/StatusSignals/robot.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python3 +""" + This is a demo program for StatusSignal usage in Phoenix 6 +""" +import wpilib +from wpilib import Timer, XboxController +from phoenix6 import * + +class MyRobot(wpilib.TimedRobot): + motor: TalonFX + request: DutyCycleOut + pos: StatusSignal[rotation] + vel: StatusSignal[rotations_per_second] + timer: Timer + joystick: XboxController + + def robotInit(self): + """Robot initialization function""" + + # Keep a reference to all the motor controllers used + self.motor = TalonFX(1, "Fred") + self.request = DutyCycleOut(0) + + self.pos = self.motor.get_position() + self.vel = self.motor.get_velocity() + + self.timer = Timer() + self.timer.start() + + self.joystick = XboxController(0) + + self.motor.set_position(6) + + def robotPeriodic(self) -> None: + self.motor.set_control(self.request.with_output(self.joystick.getLeftY())) + + def teleopInit(self) -> None: + """Start signal logger for logging purposes""" + SignalLogger.start() + + def teleopPeriodic(self): + """Every 100ms, print the status of the StatusSignal""" + + if self.timer.hasElapsed(0.1): + BaseStatusSignal.refresh_all(self.pos, self.vel) + print(f"Position is {self.pos} and velocity is {self.vel} at timestamp {self.pos.all_timestamps.get_device_timestamp().time}") + +if __name__ == "__main__": + wpilib.run(MyRobot) diff --git a/python/TankDrive/.deploy_cfg b/python/TankDrive/.deploy_cfg new file mode 100644 index 00000000..e6134433 --- /dev/null +++ b/python/TankDrive/.deploy_cfg @@ -0,0 +1,3 @@ +[auth] +hostname = 7762 + diff --git a/python/TankDrive/.gitignore b/python/TankDrive/.gitignore new file mode 100644 index 00000000..a1d0392d --- /dev/null +++ b/python/TankDrive/.gitignore @@ -0,0 +1,2 @@ +ctre_sim +tests diff --git a/python/TankDrive/.installer_config b/python/TankDrive/.installer_config new file mode 100644 index 00000000..e6134433 --- /dev/null +++ b/python/TankDrive/.installer_config @@ -0,0 +1,3 @@ +[auth] +hostname = 7762 + diff --git a/python/TankDrive/robot.py b/python/TankDrive/robot.py new file mode 100644 index 00000000..cd31fa26 --- /dev/null +++ b/python/TankDrive/robot.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python3 +""" + This is a demo program for tank drive in Python with Phoenix6 +""" +import wpilib +from phoenix6 import CoreTalonFX, Follower, DutyCycleOut +from wpilib.drive import DifferentialDrive + +class MyRobot(wpilib.TimedRobot): + def robotInit(self): + """Robot initialization function""" + + # Keep a reference to all the motor controllers used + self.front_left_motor = CoreTalonFX(0, "Fred") + self.rear_left_motor = CoreTalonFX(1, "Fred") + self.front_right_motor = CoreTalonFX(2, "Fred") + self.rear_right_motor = CoreTalonFX(3, "Fred") + + # Configure the rear motors to follow the front motors + follow_left_request = Follower(master_id=0, oppose_master_direction=False) + self.rear_left_motor.set_control(follow_left_request) + + follow_right_request = Follower(master_id=2, oppose_master_direction=False) + self.rear_right_motor.set_control(follow_right_request) + + # Keep a reference to the DutyCycleOut control request to update periodically + self.left_out = DutyCycleOut(output=0, enable_foc=True, override_brake_dur_neutral=False) + self.right_out = DutyCycleOut(output=0, enable_foc=True, override_brake_dur_neutral=False) + + # Keep a reference to an Xbox Controller for teleop control + self.joy = wpilib.XboxController(0) + + def teleopPeriodic(self): + """Runs the motors with arcade drive""" + # Get throttle and wheel values for arcade drive + throttle = self.joy.getLeftY() * -1 + wheel = self.joy.getRightX() * 1 + + # Set DutyCycleOut outputs + self.left_out.output = throttle + wheel + self.right_out.output = throttle - wheel + + # And set the DutyCycleOut to the motor controllers + self.front_left_motor.set_control(self.left_out) + self.front_right_motor.set_control(self.right_out) + + +if __name__ == "__main__": + wpilib.run(MyRobot) \ No newline at end of file From 7105a2241ff53b38f497b8be75636674ff09d023 Mon Sep 17 00:00:00 2001 From: CoryNessCTR Date: Tue, 12 Dec 2023 11:36:37 -0500 Subject: [PATCH 02/16] Adds StatusSignal unit test --- python/StatusSignals/.gitignore | 3 +- python/StatusSignals/robot.py | 4 ++ python/StatusSignals/tests/pyfrc_test.py | 47 ++++++++++++++++++++++++ 3 files changed, 53 insertions(+), 1 deletion(-) create mode 100644 python/StatusSignals/tests/pyfrc_test.py diff --git a/python/StatusSignals/.gitignore b/python/StatusSignals/.gitignore index a1d0392d..692d33ac 100644 --- a/python/StatusSignals/.gitignore +++ b/python/StatusSignals/.gitignore @@ -1,2 +1,3 @@ ctre_sim -tests +__pycache__ +.pytest_cache diff --git a/python/StatusSignals/robot.py b/python/StatusSignals/robot.py index 95ecc35a..a72ce28f 100644 --- a/python/StatusSignals/robot.py +++ b/python/StatusSignals/robot.py @@ -32,6 +32,7 @@ def robotInit(self): self.motor.set_position(6) def robotPeriodic(self) -> None: + # Drive the motor so we have a changing position/velocity self.motor.set_control(self.request.with_output(self.joystick.getLeftY())) def teleopInit(self) -> None: @@ -45,5 +46,8 @@ def teleopPeriodic(self): BaseStatusSignal.refresh_all(self.pos, self.vel) print(f"Position is {self.pos} and velocity is {self.vel} at timestamp {self.pos.all_timestamps.get_device_timestamp().time}") + latency_compensated_pos = BaseStatusSignal.get_latency_compensated_value(self.pos, self.vel) + print(f"Latency compensated position is {latency_compensated_pos}") + if __name__ == "__main__": wpilib.run(MyRobot) diff --git a/python/StatusSignals/tests/pyfrc_test.py b/python/StatusSignals/tests/pyfrc_test.py new file mode 100644 index 00000000..31dd03e6 --- /dev/null +++ b/python/StatusSignals/tests/pyfrc_test.py @@ -0,0 +1,47 @@ +''' + This test module imports tests that come with pyfrc, and can be used + to test basic functionality of just about any robot. +''' + +from time import sleep +from pyfrc.tests import * +from phoenix6 import TalonFX, StatusSignal, BaseStatusSignal + +def assert_almost_equal(a: float, b: float, range_val: float): + """ + Assert that a is within range of b + """ + assert a >= (b - range_val) and a <= (b + range_val) + +def test_latency_compensation(): + """ + Verifies that latency compensation is performing as expected + """ + fx = TalonFX(1, "sim") + pos = fx.get_position() + vel = fx.get_velocity() + + POS = 1.5 + VEL = 10 + DELAY = 0.1 + + BaseStatusSignal.wait_for_all(1.0, pos, vel) + fx.set_position(0, 1.0) # Set position to 0 to initialize + + # Position at 1.5 rotations rotating at 10 rotations per second + fx.sim_state.set_raw_rotor_position(POS) + fx.sim_state.set_rotor_velocity(VEL) + + # Wait for the signals to update + BaseStatusSignal.wait_for_all(1.0, pos, vel) + + # Wait another 100ms so we latency-compensate one full rotation + sleep(DELAY) + + # Calculate latency-compensated position + latency_compensated_pos = BaseStatusSignal.get_latency_compensated_value(pos, vel) + + assert_almost_equal(pos.value, POS, 0.01) + assert_almost_equal(vel.value, VEL, 0.01) + # This can be more loose since it's time-dependent + assert_almost_equal(latency_compensated_pos, POS + (VEL * DELAY), 0.1) From aab3e2fadc2b09275b037314f84cdc1cc423d400 Mon Sep 17 00:00:00 2001 From: CoryNessCTR Date: Tue, 12 Dec 2023 13:31:17 -0500 Subject: [PATCH 03/16] Rename ArcadeDrive & fix up --- python/{TankDrive => ArcadeDrive}/.deploy_cfg | 0 python/ArcadeDrive/.gitignore | 3 ++ .../.installer_config | 0 python/{TankDrive => ArcadeDrive}/robot.py | 30 ++++++++----------- python/ArcadeDrive/tests/pyfrc_test.py | 6 ++++ python/TankDrive/.gitignore | 2 -- 6 files changed, 22 insertions(+), 19 deletions(-) rename python/{TankDrive => ArcadeDrive}/.deploy_cfg (100%) create mode 100644 python/ArcadeDrive/.gitignore rename python/{TankDrive => ArcadeDrive}/.installer_config (100%) rename python/{TankDrive => ArcadeDrive}/robot.py (50%) create mode 100644 python/ArcadeDrive/tests/pyfrc_test.py delete mode 100644 python/TankDrive/.gitignore diff --git a/python/TankDrive/.deploy_cfg b/python/ArcadeDrive/.deploy_cfg similarity index 100% rename from python/TankDrive/.deploy_cfg rename to python/ArcadeDrive/.deploy_cfg diff --git a/python/ArcadeDrive/.gitignore b/python/ArcadeDrive/.gitignore new file mode 100644 index 00000000..692d33ac --- /dev/null +++ b/python/ArcadeDrive/.gitignore @@ -0,0 +1,3 @@ +ctre_sim +__pycache__ +.pytest_cache diff --git a/python/TankDrive/.installer_config b/python/ArcadeDrive/.installer_config similarity index 100% rename from python/TankDrive/.installer_config rename to python/ArcadeDrive/.installer_config diff --git a/python/TankDrive/robot.py b/python/ArcadeDrive/robot.py similarity index 50% rename from python/TankDrive/robot.py rename to python/ArcadeDrive/robot.py index cd31fa26..77adbdef 100644 --- a/python/TankDrive/robot.py +++ b/python/ArcadeDrive/robot.py @@ -3,29 +3,29 @@ This is a demo program for tank drive in Python with Phoenix6 """ import wpilib -from phoenix6 import CoreTalonFX, Follower, DutyCycleOut -from wpilib.drive import DifferentialDrive +from phoenix6 import TalonFX, Follower, DutyCycleOut class MyRobot(wpilib.TimedRobot): def robotInit(self): """Robot initialization function""" # Keep a reference to all the motor controllers used - self.front_left_motor = CoreTalonFX(0, "Fred") - self.rear_left_motor = CoreTalonFX(1, "Fred") - self.front_right_motor = CoreTalonFX(2, "Fred") - self.rear_right_motor = CoreTalonFX(3, "Fred") + canivore_name = "canivore" + self.front_left_motor = TalonFX(0, canivore_name) + self.rear_left_motor = TalonFX(1, canivore_name) + self.front_right_motor = TalonFX(2, canivore_name) + self.rear_right_motor = TalonFX(3, canivore_name) # Configure the rear motors to follow the front motors - follow_left_request = Follower(master_id=0, oppose_master_direction=False) + follow_left_request = Follower(0, False) self.rear_left_motor.set_control(follow_left_request) - follow_right_request = Follower(master_id=2, oppose_master_direction=False) + follow_right_request = Follower(2, False) self.rear_right_motor.set_control(follow_right_request) # Keep a reference to the DutyCycleOut control request to update periodically - self.left_out = DutyCycleOut(output=0, enable_foc=True, override_brake_dur_neutral=False) - self.right_out = DutyCycleOut(output=0, enable_foc=True, override_brake_dur_neutral=False) + self.left_out = DutyCycleOut(output=0) + self.right_out = DutyCycleOut(output=0) # Keep a reference to an Xbox Controller for teleop control self.joy = wpilib.XboxController(0) @@ -36,14 +36,10 @@ def teleopPeriodic(self): throttle = self.joy.getLeftY() * -1 wheel = self.joy.getRightX() * 1 - # Set DutyCycleOut outputs - self.left_out.output = throttle + wheel - self.right_out.output = throttle - wheel - # And set the DutyCycleOut to the motor controllers - self.front_left_motor.set_control(self.left_out) - self.front_right_motor.set_control(self.right_out) + self.front_left_motor.set_control(self.left_out.with_output(throttle + wheel)) + self.front_right_motor.set_control(self.right_out.with_output(throttle - wheel)) if __name__ == "__main__": - wpilib.run(MyRobot) \ No newline at end of file + wpilib.run(MyRobot) diff --git a/python/ArcadeDrive/tests/pyfrc_test.py b/python/ArcadeDrive/tests/pyfrc_test.py new file mode 100644 index 00000000..90ae6731 --- /dev/null +++ b/python/ArcadeDrive/tests/pyfrc_test.py @@ -0,0 +1,6 @@ +''' + This test module imports tests that come with pyfrc, and can be used + to test basic functionality of just about any robot. +''' + +from pyfrc.tests import * diff --git a/python/TankDrive/.gitignore b/python/TankDrive/.gitignore deleted file mode 100644 index a1d0392d..00000000 --- a/python/TankDrive/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -ctre_sim -tests From cd6eca5c556ffaf93937c9fa19bc75a5d9393eb5 Mon Sep 17 00:00:00 2001 From: CoryNessCTR Date: Tue, 12 Dec 2023 14:13:24 -0500 Subject: [PATCH 04/16] Add CANcoder example & fixup some pylint issues --- python/ArcadeDrive/robot.py | 14 ++++++++ python/CANcoder/.deploy_cfg | 3 ++ python/CANcoder/.gitignore | 3 ++ python/CANcoder/.installer_config | 3 ++ python/CANcoder/robot.py | 46 ++++++++++++++++++++++++ python/CANcoder/tests/pyfrc_test.py | 31 ++++++++++++++++ python/StatusSignals/robot.py | 29 ++++++++++++--- python/StatusSignals/tests/pyfrc_test.py | 10 +++--- 8 files changed, 130 insertions(+), 9 deletions(-) create mode 100644 python/CANcoder/.deploy_cfg create mode 100644 python/CANcoder/.gitignore create mode 100644 python/CANcoder/.installer_config create mode 100644 python/CANcoder/robot.py create mode 100644 python/CANcoder/tests/pyfrc_test.py diff --git a/python/ArcadeDrive/robot.py b/python/ArcadeDrive/robot.py index 77adbdef..8d637512 100644 --- a/python/ArcadeDrive/robot.py +++ b/python/ArcadeDrive/robot.py @@ -5,7 +5,21 @@ import wpilib from phoenix6 import TalonFX, Follower, DutyCycleOut + class MyRobot(wpilib.TimedRobot): + """ + Example program that shows to do simple arcade drive in robotpy + with Phoenix 6 + """ + + front_left_motor: TalonFX + rear_left_motor: TalonFX + front_right_motor: TalonFX + rear_right_motor: TalonFX + left_out: DutyCycleOut + right_out: DutyCycleOut + joy: wpilib.XboxController + def robotInit(self): """Robot initialization function""" diff --git a/python/CANcoder/.deploy_cfg b/python/CANcoder/.deploy_cfg new file mode 100644 index 00000000..e6134433 --- /dev/null +++ b/python/CANcoder/.deploy_cfg @@ -0,0 +1,3 @@ +[auth] +hostname = 7762 + diff --git a/python/CANcoder/.gitignore b/python/CANcoder/.gitignore new file mode 100644 index 00000000..692d33ac --- /dev/null +++ b/python/CANcoder/.gitignore @@ -0,0 +1,3 @@ +ctre_sim +__pycache__ +.pytest_cache diff --git a/python/CANcoder/.installer_config b/python/CANcoder/.installer_config new file mode 100644 index 00000000..e6134433 --- /dev/null +++ b/python/CANcoder/.installer_config @@ -0,0 +1,3 @@ +[auth] +hostname = 7762 + diff --git a/python/CANcoder/robot.py b/python/CANcoder/robot.py new file mode 100644 index 00000000..f10d7f5c --- /dev/null +++ b/python/CANcoder/robot.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python3 +""" + This is a demo program for StatusSignal usage in Phoenix 6 +""" +import wpilib +from wpilib import Timer +from phoenix6 import CANcoder + +class MyRobot(wpilib.TimedRobot): + """ + Example program that shows how to use CANcoder + in Phoenix 6 python + """ + + cancoder: CANcoder + timer: Timer + + def robotInit(self): + """Robot initialization function""" + + # Keep a reference to all the motor controllers used + self.cancoder = CANcoder(1, "canivore") + + self.timer = Timer() + self.timer.start() + + def teleopPeriodic(self): + """Every 100ms, print the status of the StatusSignal""" + + if self.timer.hasElapsed(0.1): + # get_position automatically calls refresh(), no need to manually refresh. + # + # StatusSignals also implement the str dunder to provide a useful print of the signal + pos = self.cancoder.get_position() + print(f"Positions is {str(pos)} with {pos.timestamp.get_latency()} seconds of latency") + + # Get the velocity StatusSignal + vel = self.cancoder.get_velocity() + # This time wait for the signal to reduce latency + vel.wait_for_update(0.1) + print(f"Velocity is {vel} with {vel.timestamp.get_latency()} seconds of latency") + + print("") + +if __name__ == "__main__": + wpilib.run(MyRobot) diff --git a/python/CANcoder/tests/pyfrc_test.py b/python/CANcoder/tests/pyfrc_test.py new file mode 100644 index 00000000..adbc4ab8 --- /dev/null +++ b/python/CANcoder/tests/pyfrc_test.py @@ -0,0 +1,31 @@ +''' + This test module imports tests that come with pyfrc, and can be used + to test basic functionality of just about any robot. +''' + +from pyfrc.tests import * +from phoenix6 import CANcoder, CANcoderConfiguration + +FIRST_SET = 0 +SECOND_SET = 4.8 + +def assert_almost_equal(a: float, b: float, range_val: float): + """ + Assert that a is within range of b + """ + assert a >= (b - range_val) and a <= (b + range_val) + +def test_pos_setter(): + cancoder = CANcoder(1, "sim") + pos = cancoder.get_position() + + cfg = CANcoderConfiguration() + cancoder.configurator.apply(cfg) + + cancoder.set_position(FIRST_SET) + pos.wait_for_update(1.0) + assert_almost_equal(pos.value, FIRST_SET, 0.01) + + cancoder.set_position(SECOND_SET) + pos.wait_for_update(1.0) + assert_almost_equal(pos.value, SECOND_SET, 0.01) diff --git a/python/StatusSignals/robot.py b/python/StatusSignals/robot.py index a72ce28f..2425011a 100644 --- a/python/StatusSignals/robot.py +++ b/python/StatusSignals/robot.py @@ -4,9 +4,23 @@ """ import wpilib from wpilib import Timer, XboxController -from phoenix6 import * +from phoenix6 import ( + TalonFX, + DutyCycleOut, + StatusSignal, + rotation, + rotations_per_second, + SignalLogger, + BaseStatusSignal, +) + class MyRobot(wpilib.TimedRobot): + """ + Example program that provides basic usage on StatusSignals + in Phoenix 6 python + """ + motor: TalonFX request: DutyCycleOut pos: StatusSignal[rotation] @@ -18,7 +32,7 @@ def robotInit(self): """Robot initialization function""" # Keep a reference to all the motor controllers used - self.motor = TalonFX(1, "Fred") + self.motor = TalonFX(1, "canivore") self.request = DutyCycleOut(0) self.pos = self.motor.get_position() @@ -44,10 +58,17 @@ def teleopPeriodic(self): if self.timer.hasElapsed(0.1): BaseStatusSignal.refresh_all(self.pos, self.vel) - print(f"Position is {self.pos} and velocity is {self.vel} at timestamp {self.pos.all_timestamps.get_device_timestamp().time}") - latency_compensated_pos = BaseStatusSignal.get_latency_compensated_value(self.pos, self.vel) + pos_timestamp = self.pos.all_timestamps.get_device_timestamp().time + print( + f"Position is {self.pos} and velocity is {self.vel} at timestamp {pos_timestamp}" + ) + + latency_compensated_pos = BaseStatusSignal.get_latency_compensated_value( + self.pos, self.vel + ) print(f"Latency compensated position is {latency_compensated_pos}") + if __name__ == "__main__": wpilib.run(MyRobot) diff --git a/python/StatusSignals/tests/pyfrc_test.py b/python/StatusSignals/tests/pyfrc_test.py index 31dd03e6..00462b26 100644 --- a/python/StatusSignals/tests/pyfrc_test.py +++ b/python/StatusSignals/tests/pyfrc_test.py @@ -5,7 +5,11 @@ from time import sleep from pyfrc.tests import * -from phoenix6 import TalonFX, StatusSignal, BaseStatusSignal +from phoenix6 import TalonFX, BaseStatusSignal + +POS = 1.5 +VEL = 10 +DELAY = 0.1 def assert_almost_equal(a: float, b: float, range_val: float): """ @@ -21,10 +25,6 @@ def test_latency_compensation(): pos = fx.get_position() vel = fx.get_velocity() - POS = 1.5 - VEL = 10 - DELAY = 0.1 - BaseStatusSignal.wait_for_all(1.0, pos, vel) fx.set_position(0, 1.0) # Set position to 0 to initialize From 2a9d2fffd35704b877d201d831f5383a358bafc1 Mon Sep 17 00:00:00 2001 From: CoryNessCTR Date: Tue, 12 Dec 2023 14:26:14 -0500 Subject: [PATCH 05/16] Include CI for python --- .github/workflows/build-all-parallel.yml | 32 ++++++++++++++++++++- build_parallel_workflow.py | 36 ++++++++++++++++++++++-- 2 files changed, 65 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-all-parallel.yml b/.github/workflows/build-all-parallel.yml index a3f4c4cd..f13159a3 100644 --- a/.github/workflows/build-all-parallel.yml +++ b/.github/workflows/build-all-parallel.yml @@ -81,7 +81,7 @@ jobs: container: wpilib/roborio-cross-ubuntu:2024-22.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 # Grant execute permission for gradlew - name: Grant execute permission for gradlew @@ -90,3 +90,33 @@ jobs: # Runs a single command using the runners shell - name: Compile and run tests on robot code for project ${{ matrix.project-name }} run: cd "${{ matrix.directory }}" && ./gradlew build + + build-python: + + strategy: + fail-fast: false + matrix: + include: + - project-name: 'ArcadeDrive' + directory: 'python/ArcadeDrive' + - project-name: 'CANcoder' + directory: 'python/CANcoder' + - project-name: 'StatusSignals' + directory: 'python/StatusSignals' + python_version: ['3.9', '3.10', '3.11', '3.12'] + os: ['ubuntu-22.04', 'macos-12', 'windows-2022'] + + runs-on: ${{ matrix.os }} + + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v4 + with: + python-version: ${{ matrix.python_version }} + - name: Install python dependencies + run: | + pip install -U pip + pip install 'robotpy' phoenix6 + - name: Test ${{ matrix.project-name }} + run: | + cd "${{ matrix.directory }}" && python3 robot.py test diff --git a/build_parallel_workflow.py b/build_parallel_workflow.py index 1cc0c0ac..8497595c 100644 --- a/build_parallel_workflow.py +++ b/build_parallel_workflow.py @@ -31,7 +31,7 @@ container: wpilib/roborio-cross-ubuntu:2024-22.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 # Grant execute permission for gradlew - name: Grant execute permission for gradlew @@ -40,6 +40,30 @@ # Runs a single command using the runners shell - name: Compile and run tests on robot code for project ${{{{ matrix.project-name }}}} run: cd "${{{{ matrix.directory }}}}" && ./gradlew build + + build-python: + + strategy: + fail-fast: false + matrix: + include:{python_projects_as_matrix} + python_version: ['3.9', '3.10', '3.11', '3.12'] + os: ['ubuntu-22.04', 'macos-12', 'windows-2022'] + + runs-on: ${{{{ matrix.os }}}} + + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v4 + with: + python-version: ${{{{ matrix.python_version }}}} + - name: Install python dependencies + run: | + pip install -U pip + pip install 'robotpy' phoenix6 + - name: Test ${{{{ matrix.project-name }}}} + run: | + cd "${{{{ matrix.directory }}}}" && python3 robot.py test """ PROJECT_MATRIX_TEMPLATE = """ @@ -47,6 +71,7 @@ directory: '{project_dir}'""" PROJECTS_TO_SEARCH = ["cpp", "java"] +PYTHON_PROJECTS_TO_SEARCH = ["python"] project_matrix = [] @@ -54,6 +79,13 @@ # Find every project in here and build up an array of strings to generate the workflow file for project in os.listdir(project_dir): project_matrix.append(PROJECT_MATRIX_TEMPLATE.format(project_name=project, project_dir=f"{project_dir}/{project}")) + +python_project_matrix = [] +for project_dir in PYTHON_PROJECTS_TO_SEARCH: + # Find every project in here and build up an array of strings to generate the workflow file + for project in os.listdir(project_dir): + python_project_matrix.append(PROJECT_MATRIX_TEMPLATE.format(project_name=project, project_dir=f"{project_dir}/{project}")) with open(".github/workflows/build-all-parallel.yml", "w", encoding="utf-8") as workflow_file: - workflow_file.write(WORKFLOW_TEMPLATE.format(projects_as_matrix="".join(project_matrix))) + workflow_file.write(WORKFLOW_TEMPLATE.format(projects_as_matrix="".join(project_matrix), + python_projects_as_matrix="".join(python_project_matrix))) From 7949daaf1b77c226685f8c9a42250c161919f9e2 Mon Sep 17 00:00:00 2001 From: CoryNessCTR Date: Tue, 12 Dec 2023 14:37:41 -0500 Subject: [PATCH 06/16] Loosen the status_signal tests --- python/StatusSignals/tests/pyfrc_test.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/python/StatusSignals/tests/pyfrc_test.py b/python/StatusSignals/tests/pyfrc_test.py index 00462b26..b730eb9d 100644 --- a/python/StatusSignals/tests/pyfrc_test.py +++ b/python/StatusSignals/tests/pyfrc_test.py @@ -40,8 +40,11 @@ def test_latency_compensation(): # Calculate latency-compensated position latency_compensated_pos = BaseStatusSignal.get_latency_compensated_value(pos, vel) + measured_latency = pos.timestamp.get_latency() assert_almost_equal(pos.value, POS, 0.01) assert_almost_equal(vel.value, VEL, 0.01) # This can be more loose since it's time-dependent - assert_almost_equal(latency_compensated_pos, POS + (VEL * DELAY), 0.1) + assert_almost_equal(measured_latency, DELAY, 0.1) + # This is also a little more loose since measured_latency can be different from the latency used + assert_almost_equal(latency_compensated_pos, POS + (VEL * measured_latency), 0.1) From f5f6731962916f3a29d3b002359da4c59ad1ddf5 Mon Sep 17 00:00:00 2001 From: CoryNessCTR Date: Tue, 12 Dec 2023 14:46:42 -0500 Subject: [PATCH 07/16] Properly build out the matrix --- .github/workflows/build-all-parallel.yml | 10 ++-------- build_parallel_workflow.py | 10 ++++++---- 2 files changed, 8 insertions(+), 12 deletions(-) diff --git a/.github/workflows/build-all-parallel.yml b/.github/workflows/build-all-parallel.yml index f13159a3..1286a54a 100644 --- a/.github/workflows/build-all-parallel.yml +++ b/.github/workflows/build-all-parallel.yml @@ -96,15 +96,9 @@ jobs: strategy: fail-fast: false matrix: - include: - - project-name: 'ArcadeDrive' - directory: 'python/ArcadeDrive' - - project-name: 'CANcoder' - directory: 'python/CANcoder' - - project-name: 'StatusSignals' - directory: 'python/StatusSignals' python_version: ['3.9', '3.10', '3.11', '3.12'] os: ['ubuntu-22.04', 'macos-12', 'windows-2022'] + project-name: ['ArcadeDrive', 'CANcoder', 'StatusSignals'] runs-on: ${{ matrix.os }} @@ -119,4 +113,4 @@ jobs: pip install 'robotpy' phoenix6 - name: Test ${{ matrix.project-name }} run: | - cd "${{ matrix.directory }}" && python3 robot.py test + cd "python/${{ matrix.project-name }}" && python3 robot.py test diff --git a/build_parallel_workflow.py b/build_parallel_workflow.py index 8497595c..9b12b31a 100644 --- a/build_parallel_workflow.py +++ b/build_parallel_workflow.py @@ -46,9 +46,9 @@ strategy: fail-fast: false matrix: - include:{python_projects_as_matrix} python_version: ['3.9', '3.10', '3.11', '3.12'] os: ['ubuntu-22.04', 'macos-12', 'windows-2022'] + project-name: [{python_projects}] runs-on: ${{{{ matrix.os }}}} @@ -63,13 +63,15 @@ pip install 'robotpy' phoenix6 - name: Test ${{{{ matrix.project-name }}}} run: | - cd "${{{{ matrix.directory }}}}" && python3 robot.py test + cd "python/${{{{ matrix.project-name }}}}" && python3 robot.py test """ PROJECT_MATRIX_TEMPLATE = """ - project-name: '{project_name}' directory: '{project_dir}'""" +MATRIX_TEMPLATE = "'{project_name}'" + PROJECTS_TO_SEARCH = ["cpp", "java"] PYTHON_PROJECTS_TO_SEARCH = ["python"] @@ -84,8 +86,8 @@ for project_dir in PYTHON_PROJECTS_TO_SEARCH: # Find every project in here and build up an array of strings to generate the workflow file for project in os.listdir(project_dir): - python_project_matrix.append(PROJECT_MATRIX_TEMPLATE.format(project_name=project, project_dir=f"{project_dir}/{project}")) + python_project_matrix.append(MATRIX_TEMPLATE.format(project_name=project)) with open(".github/workflows/build-all-parallel.yml", "w", encoding="utf-8") as workflow_file: workflow_file.write(WORKFLOW_TEMPLATE.format(projects_as_matrix="".join(project_matrix), - python_projects_as_matrix="".join(python_project_matrix))) + python_projects=", ".join(python_project_matrix))) From b8e70444a4912242d24ffd9f97361214e19da087 Mon Sep 17 00:00:00 2001 From: CoryNessCTR Date: Tue, 12 Dec 2023 15:06:45 -0500 Subject: [PATCH 08/16] Remove 3.12 & latency check for statussignal test --- .github/workflows/build-all-parallel.yml | 2 +- build_parallel_workflow.py | 2 +- python/StatusSignals/tests/pyfrc_test.py | 2 -- 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build-all-parallel.yml b/.github/workflows/build-all-parallel.yml index 1286a54a..c4495bdb 100644 --- a/.github/workflows/build-all-parallel.yml +++ b/.github/workflows/build-all-parallel.yml @@ -96,7 +96,7 @@ jobs: strategy: fail-fast: false matrix: - python_version: ['3.9', '3.10', '3.11', '3.12'] + python_version: ['3.9', '3.10', '3.11'] os: ['ubuntu-22.04', 'macos-12', 'windows-2022'] project-name: ['ArcadeDrive', 'CANcoder', 'StatusSignals'] diff --git a/build_parallel_workflow.py b/build_parallel_workflow.py index 9b12b31a..62e88865 100644 --- a/build_parallel_workflow.py +++ b/build_parallel_workflow.py @@ -46,7 +46,7 @@ strategy: fail-fast: false matrix: - python_version: ['3.9', '3.10', '3.11', '3.12'] + python_version: ['3.9', '3.10', '3.11'] os: ['ubuntu-22.04', 'macos-12', 'windows-2022'] project-name: [{python_projects}] diff --git a/python/StatusSignals/tests/pyfrc_test.py b/python/StatusSignals/tests/pyfrc_test.py index b730eb9d..aa9e48b5 100644 --- a/python/StatusSignals/tests/pyfrc_test.py +++ b/python/StatusSignals/tests/pyfrc_test.py @@ -45,6 +45,4 @@ def test_latency_compensation(): assert_almost_equal(pos.value, POS, 0.01) assert_almost_equal(vel.value, VEL, 0.01) # This can be more loose since it's time-dependent - assert_almost_equal(measured_latency, DELAY, 0.1) - # This is also a little more loose since measured_latency can be different from the latency used assert_almost_equal(latency_compensated_pos, POS + (VEL * measured_latency), 0.1) From 63b5700bb698252d9f58b686e1520a7cd4c348db Mon Sep 17 00:00:00 2001 From: CoryNessCTR Date: Tue, 12 Dec 2023 16:48:36 -0500 Subject: [PATCH 09/16] Small tweaks Now that I'm using pylance and not pylint, I can be more liberal in the examples, also --- python/ArcadeDrive/robot.py | 10 +--------- python/CANcoder/robot.py | 5 +---- python/StatusSignals/robot.py | 14 +------------- 3 files changed, 3 insertions(+), 26 deletions(-) diff --git a/python/ArcadeDrive/robot.py b/python/ArcadeDrive/robot.py index 8d637512..264f4ce8 100644 --- a/python/ArcadeDrive/robot.py +++ b/python/ArcadeDrive/robot.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 """ - This is a demo program for tank drive in Python with Phoenix6 + This is a demo program for arcade drive in Python with Phoenix 6 """ import wpilib from phoenix6 import TalonFX, Follower, DutyCycleOut @@ -12,14 +12,6 @@ class MyRobot(wpilib.TimedRobot): with Phoenix 6 """ - front_left_motor: TalonFX - rear_left_motor: TalonFX - front_right_motor: TalonFX - rear_right_motor: TalonFX - left_out: DutyCycleOut - right_out: DutyCycleOut - joy: wpilib.XboxController - def robotInit(self): """Robot initialization function""" diff --git a/python/CANcoder/robot.py b/python/CANcoder/robot.py index f10d7f5c..0a974fa8 100644 --- a/python/CANcoder/robot.py +++ b/python/CANcoder/robot.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 """ - This is a demo program for StatusSignal usage in Phoenix 6 + This is a demo program for CANcoder usage in Phoenix 6 """ import wpilib from wpilib import Timer @@ -11,9 +11,6 @@ class MyRobot(wpilib.TimedRobot): Example program that shows how to use CANcoder in Phoenix 6 python """ - - cancoder: CANcoder - timer: Timer def robotInit(self): """Robot initialization function""" diff --git a/python/StatusSignals/robot.py b/python/StatusSignals/robot.py index 2425011a..f88a99a1 100644 --- a/python/StatusSignals/robot.py +++ b/python/StatusSignals/robot.py @@ -7,9 +7,6 @@ from phoenix6 import ( TalonFX, DutyCycleOut, - StatusSignal, - rotation, - rotations_per_second, SignalLogger, BaseStatusSignal, ) @@ -21,13 +18,6 @@ class MyRobot(wpilib.TimedRobot): in Phoenix 6 python """ - motor: TalonFX - request: DutyCycleOut - pos: StatusSignal[rotation] - vel: StatusSignal[rotations_per_second] - timer: Timer - joystick: XboxController - def robotInit(self): """Robot initialization function""" @@ -60,9 +50,7 @@ def teleopPeriodic(self): BaseStatusSignal.refresh_all(self.pos, self.vel) pos_timestamp = self.pos.all_timestamps.get_device_timestamp().time - print( - f"Position is {self.pos} and velocity is {self.vel} at timestamp {pos_timestamp}" - ) + print(f"Position is {self.pos} and velocity is {self.vel} at timestamp {pos_timestamp}") latency_compensated_pos = BaseStatusSignal.get_latency_compensated_value( self.pos, self.vel From 7b77d356366603497140f4b75d854419145bf863 Mon Sep 17 00:00:00 2001 From: CoryNessCTR Date: Tue, 12 Dec 2023 17:20:25 -0500 Subject: [PATCH 10/16] Adds TalonFX and Position PID examples --- python/PositionClosedLoop/.deploy_cfg | 3 + python/PositionClosedLoop/.gitignore | 3 + python/PositionClosedLoop/.installer_config | 3 + python/PositionClosedLoop/robot.py | 55 ++++++++++++++++ python/PositionClosedLoop/tests/pyfrc_test.py | 63 +++++++++++++++++++ python/StatusSignals/tests/pyfrc_test.py | 3 +- python/TalonFX/.deploy_cfg | 3 + python/TalonFX/.gitignore | 3 + python/TalonFX/.installer_config | 3 + python/TalonFX/robot.py | 43 +++++++++++++ python/TalonFX/tests/pyfrc_test.py | 31 +++++++++ 11 files changed, 212 insertions(+), 1 deletion(-) create mode 100644 python/PositionClosedLoop/.deploy_cfg create mode 100644 python/PositionClosedLoop/.gitignore create mode 100644 python/PositionClosedLoop/.installer_config create mode 100644 python/PositionClosedLoop/robot.py create mode 100644 python/PositionClosedLoop/tests/pyfrc_test.py create mode 100644 python/TalonFX/.deploy_cfg create mode 100644 python/TalonFX/.gitignore create mode 100644 python/TalonFX/.installer_config create mode 100644 python/TalonFX/robot.py create mode 100644 python/TalonFX/tests/pyfrc_test.py diff --git a/python/PositionClosedLoop/.deploy_cfg b/python/PositionClosedLoop/.deploy_cfg new file mode 100644 index 00000000..e6134433 --- /dev/null +++ b/python/PositionClosedLoop/.deploy_cfg @@ -0,0 +1,3 @@ +[auth] +hostname = 7762 + diff --git a/python/PositionClosedLoop/.gitignore b/python/PositionClosedLoop/.gitignore new file mode 100644 index 00000000..692d33ac --- /dev/null +++ b/python/PositionClosedLoop/.gitignore @@ -0,0 +1,3 @@ +ctre_sim +__pycache__ +.pytest_cache diff --git a/python/PositionClosedLoop/.installer_config b/python/PositionClosedLoop/.installer_config new file mode 100644 index 00000000..e6134433 --- /dev/null +++ b/python/PositionClosedLoop/.installer_config @@ -0,0 +1,3 @@ +[auth] +hostname = 7762 + diff --git a/python/PositionClosedLoop/robot.py b/python/PositionClosedLoop/robot.py new file mode 100644 index 00000000..d59ea547 --- /dev/null +++ b/python/PositionClosedLoop/robot.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python3 +""" + This is a demo program for TalonFX Position PID usage in Phoenix 6 +""" +import wpilib +from wpilib import Timer, XboxController +from phoenix6 import TalonFX, TalonFXConfiguration, PositionVoltage + +class MyRobot(wpilib.TimedRobot): + """ + Example program that shows how to use TalonFX + in Phoenix 6 python + """ + + def robotInit(self): + """Robot initialization function""" + + # Keep a reference to all the motor controllers used + self.talonfx = TalonFX(1, "canivore") + self.position_request = PositionVoltage(0) + + cfg = TalonFXConfiguration() + # Set PID gains + cfg.slot0.k_p = 10 + cfg.slot0.k_d = 0.2 + + # Apply PID gains to motor + self.talonfx.configurator.apply(cfg) + + self.joystick = XboxController(0) + self.timer = Timer() + self.timer.start() + + def teleopInit(self): + """Set the position of the talonfx to 0 so we know we're centered""" + self.talonfx.set_position(0) + + def teleopPeriodic(self): + """Every 100ms, print the status of the StatusSignal""" + + # Target a position of +- 10 rotations depending on joystick position + self.talonfx.set_control(self.position_request.with_position(self.joystick.getLeftY() * 10)) + + if self.timer.hasElapsed(0.1): + # Print the position & velocity to see what they are + pos = self.talonfx.get_position() + vel = self.talonfx.get_velocity() + + print(f"Position: {pos}") + print(f"Velocity: {vel}") + + print("") + +if __name__ == "__main__": + wpilib.run(MyRobot) diff --git a/python/PositionClosedLoop/tests/pyfrc_test.py b/python/PositionClosedLoop/tests/pyfrc_test.py new file mode 100644 index 00000000..82f1dbc2 --- /dev/null +++ b/python/PositionClosedLoop/tests/pyfrc_test.py @@ -0,0 +1,63 @@ +''' + This test module imports tests that come with pyfrc, and can be used + to test basic functionality of just about any robot. +''' + +from time import sleep +from pyfrc.tests import * +from phoenix6 import TalonFX, TalonFXConfiguration, PositionVoltage +from phoenix6.unmanaged import feed_enable +from wpilib.simulation import DCMotorSim +from wpimath.system.plant import DCMotor +from wpimath.units import radiansToRotations + +FIRST_SET = 0 +SECOND_SET = 4.8 + +def assert_almost_equal(a: float, b: float, range_val: float): + """ + Assert that a is within range of b + """ + assert a >= (b - range_val) and a <= (b + range_val) + +# PID loop means we should be kinda fast, let's target 10ms +LOOP_PERIOD = 0.01 +def wait_with_sim(time: float, fx: TalonFX, dcmotorsim: DCMotorSim): + start_time = 0 + while start_time < time: + feed_enable(LOOP_PERIOD * 2) + start_time += LOOP_PERIOD + + dcmotorsim.setInputVoltage(fx.sim_state.motor_voltage) + dcmotorsim.update(LOOP_PERIOD) + fx.sim_state.set_raw_rotor_position(radiansToRotations(dcmotorsim.getAngularPosition())) + fx.sim_state.set_rotor_velocity(radiansToRotations(dcmotorsim.getAngularVelocity())) + + sleep(LOOP_PERIOD) + +def test_position_closed_loop(): + talonfx = TalonFX(1, "sim") + motorsim = DCMotorSim(DCMotor.falcon500FOC(1), 1.0, 0.001) + pos = talonfx.get_position() + + talonfx.sim_state.set_raw_rotor_position(radiansToRotations(motorsim.getAngularPosition())) + talonfx.sim_state.set_supply_voltage(12) + + cfg = TalonFXConfiguration() + cfg.slot0.k_p = 2 + cfg.slot0.k_d = 0.1 + assert talonfx.configurator.apply(cfg).is_ok() + assert talonfx.set_position(FIRST_SET).is_ok() + + pos.wait_for_update(1) + assert_almost_equal(pos.value, 0, 0.01) + + # Closed loop for 1 seconds to a target of 1 rotation, and verify we're close + target_control = PositionVoltage(position=1.0) + talonfx.set_control(target_control) + + wait_with_sim(1, talonfx, motorsim) + + # Verify position is close to target + pos.wait_for_update(1) + assert_almost_equal(pos.value, 1, 0.01) diff --git a/python/StatusSignals/tests/pyfrc_test.py b/python/StatusSignals/tests/pyfrc_test.py index aa9e48b5..c5392747 100644 --- a/python/StatusSignals/tests/pyfrc_test.py +++ b/python/StatusSignals/tests/pyfrc_test.py @@ -32,7 +32,8 @@ def test_latency_compensation(): fx.sim_state.set_raw_rotor_position(POS) fx.sim_state.set_rotor_velocity(VEL) - # Wait for the signals to update + # Wait for 2 signals to update to ensure we have a fresh signal + BaseStatusSignal.wait_for_all(1.0, pos, vel) BaseStatusSignal.wait_for_all(1.0, pos, vel) # Wait another 100ms so we latency-compensate one full rotation diff --git a/python/TalonFX/.deploy_cfg b/python/TalonFX/.deploy_cfg new file mode 100644 index 00000000..e6134433 --- /dev/null +++ b/python/TalonFX/.deploy_cfg @@ -0,0 +1,3 @@ +[auth] +hostname = 7762 + diff --git a/python/TalonFX/.gitignore b/python/TalonFX/.gitignore new file mode 100644 index 00000000..692d33ac --- /dev/null +++ b/python/TalonFX/.gitignore @@ -0,0 +1,3 @@ +ctre_sim +__pycache__ +.pytest_cache diff --git a/python/TalonFX/.installer_config b/python/TalonFX/.installer_config new file mode 100644 index 00000000..e6134433 --- /dev/null +++ b/python/TalonFX/.installer_config @@ -0,0 +1,3 @@ +[auth] +hostname = 7762 + diff --git a/python/TalonFX/robot.py b/python/TalonFX/robot.py new file mode 100644 index 00000000..5bc846fe --- /dev/null +++ b/python/TalonFX/robot.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 +""" + This is a demo program for TalonFX usage in Phoenix 6 +""" +import wpilib +from wpilib import Timer +from phoenix6 import TalonFX + +class MyRobot(wpilib.TimedRobot): + """ + Example program that shows how to use TalonFX + in Phoenix 6 python + """ + + def robotInit(self): + """Robot initialization function""" + + # Keep a reference to all the motor controllers used + self.talonfx = TalonFX(1, "canivore") + + self.timer = Timer() + self.timer.start() + + def teleopPeriodic(self): + """Every 100ms, print the status of the StatusSignal""" + + if self.timer.hasElapsed(0.1): + # get_position automatically calls refresh(), no need to manually refresh. + # + # StatusSignals also implement the str dunder to provide a useful print of the signal + pos = self.talonfx.get_position() + print(f"Positions is {str(pos)} with {pos.timestamp.get_latency()} seconds of latency") + + # Get the velocity StatusSignal + vel = self.talonfx.get_velocity() + # This time wait for the signal to reduce latency + vel.wait_for_update(0.1) + print(f"Velocity is {vel} with {vel.timestamp.get_latency()} seconds of latency") + + print("") + +if __name__ == "__main__": + wpilib.run(MyRobot) diff --git a/python/TalonFX/tests/pyfrc_test.py b/python/TalonFX/tests/pyfrc_test.py new file mode 100644 index 00000000..b8adee14 --- /dev/null +++ b/python/TalonFX/tests/pyfrc_test.py @@ -0,0 +1,31 @@ +''' + This test module imports tests that come with pyfrc, and can be used + to test basic functionality of just about any robot. +''' + +from pyfrc.tests import * +from phoenix6 import TalonFX, TalonFXConfiguration + +FIRST_SET = 0 +SECOND_SET = 4.8 + +def assert_almost_equal(a: float, b: float, range_val: float): + """ + Assert that a is within range of b + """ + assert a >= (b - range_val) and a <= (b + range_val) + +def test_pos_setter(): + talonfx = talonfx(1, "sim") + pos = talonfx.get_position() + + cfg = TalonFXConfiguration() + talonfx.configurator.apply(cfg) + + talonfx.set_position(FIRST_SET) + pos.wait_for_update(1.0) + assert_almost_equal(pos.value, FIRST_SET, 0.01) + + talonfx.set_position(SECOND_SET) + pos.wait_for_update(1.0) + assert_almost_equal(pos.value, SECOND_SET, 0.01) From 49fb71635d5d54f325c4e67937597af76ef8ba98 Mon Sep 17 00:00:00 2001 From: CoryNessCTR Date: Mon, 18 Dec 2023 14:06:39 -0500 Subject: [PATCH 11/16] Update for beta-5 --- python/ArcadeDrive/robot.py | 18 +++++++++--------- python/CANcoder/robot.py | 4 ++-- python/CANcoder/tests/pyfrc_test.py | 6 +++--- python/PositionClosedLoop/robot.py | 8 ++++---- python/PositionClosedLoop/tests/pyfrc_test.py | 10 +++++----- python/StatusSignals/robot.py | 8 ++++---- python/StatusSignals/tests/pyfrc_test.py | 4 ++-- python/TalonFX/robot.py | 4 ++-- python/TalonFX/tests/pyfrc_test.py | 6 +++--- 9 files changed, 34 insertions(+), 34 deletions(-) diff --git a/python/ArcadeDrive/robot.py b/python/ArcadeDrive/robot.py index 264f4ce8..d44ffd22 100644 --- a/python/ArcadeDrive/robot.py +++ b/python/ArcadeDrive/robot.py @@ -3,7 +3,7 @@ This is a demo program for arcade drive in Python with Phoenix 6 """ import wpilib -from phoenix6 import TalonFX, Follower, DutyCycleOut +from phoenix6 import hardware, controls class MyRobot(wpilib.TimedRobot): @@ -17,21 +17,21 @@ def robotInit(self): # Keep a reference to all the motor controllers used canivore_name = "canivore" - self.front_left_motor = TalonFX(0, canivore_name) - self.rear_left_motor = TalonFX(1, canivore_name) - self.front_right_motor = TalonFX(2, canivore_name) - self.rear_right_motor = TalonFX(3, canivore_name) + self.front_left_motor = hardware.TalonFX(0, canivore_name) + self.rear_left_motor = hardware.TalonFX(1, canivore_name) + self.front_right_motor = hardware.TalonFX(2, canivore_name) + self.rear_right_motor = hardware.TalonFX(3, canivore_name) # Configure the rear motors to follow the front motors - follow_left_request = Follower(0, False) + follow_left_request = controls.Follower(0, False) self.rear_left_motor.set_control(follow_left_request) - follow_right_request = Follower(2, False) + follow_right_request = controls.Follower(2, False) self.rear_right_motor.set_control(follow_right_request) # Keep a reference to the DutyCycleOut control request to update periodically - self.left_out = DutyCycleOut(output=0) - self.right_out = DutyCycleOut(output=0) + self.left_out = controls.DutyCycleOut(output=0) + self.right_out = controls.DutyCycleOut(output=0) # Keep a reference to an Xbox Controller for teleop control self.joy = wpilib.XboxController(0) diff --git a/python/CANcoder/robot.py b/python/CANcoder/robot.py index 0a974fa8..988068ec 100644 --- a/python/CANcoder/robot.py +++ b/python/CANcoder/robot.py @@ -4,7 +4,7 @@ """ import wpilib from wpilib import Timer -from phoenix6 import CANcoder +from phoenix6 import hardware class MyRobot(wpilib.TimedRobot): """ @@ -16,7 +16,7 @@ def robotInit(self): """Robot initialization function""" # Keep a reference to all the motor controllers used - self.cancoder = CANcoder(1, "canivore") + self.cancoder = hardware.CANcoder(1, "canivore") self.timer = Timer() self.timer.start() diff --git a/python/CANcoder/tests/pyfrc_test.py b/python/CANcoder/tests/pyfrc_test.py index adbc4ab8..4e1b650b 100644 --- a/python/CANcoder/tests/pyfrc_test.py +++ b/python/CANcoder/tests/pyfrc_test.py @@ -4,7 +4,7 @@ ''' from pyfrc.tests import * -from phoenix6 import CANcoder, CANcoderConfiguration +from phoenix6 import hardware, configs FIRST_SET = 0 SECOND_SET = 4.8 @@ -16,10 +16,10 @@ def assert_almost_equal(a: float, b: float, range_val: float): assert a >= (b - range_val) and a <= (b + range_val) def test_pos_setter(): - cancoder = CANcoder(1, "sim") + cancoder = hardware.CANcoder(1, "sim") pos = cancoder.get_position() - cfg = CANcoderConfiguration() + cfg = configs.CANcoderConfiguration() cancoder.configurator.apply(cfg) cancoder.set_position(FIRST_SET) diff --git a/python/PositionClosedLoop/robot.py b/python/PositionClosedLoop/robot.py index d59ea547..2bc38b16 100644 --- a/python/PositionClosedLoop/robot.py +++ b/python/PositionClosedLoop/robot.py @@ -4,7 +4,7 @@ """ import wpilib from wpilib import Timer, XboxController -from phoenix6 import TalonFX, TalonFXConfiguration, PositionVoltage +from phoenix6 import hardware, controls, configs class MyRobot(wpilib.TimedRobot): """ @@ -16,10 +16,10 @@ def robotInit(self): """Robot initialization function""" # Keep a reference to all the motor controllers used - self.talonfx = TalonFX(1, "canivore") - self.position_request = PositionVoltage(0) + self.talonfx = hardware.TalonFX(1, "canivore") + self.position_request = controls.PositionVoltage(0) - cfg = TalonFXConfiguration() + cfg = configs.TalonFXConfiguration() # Set PID gains cfg.slot0.k_p = 10 cfg.slot0.k_d = 0.2 diff --git a/python/PositionClosedLoop/tests/pyfrc_test.py b/python/PositionClosedLoop/tests/pyfrc_test.py index 82f1dbc2..ec1c97d1 100644 --- a/python/PositionClosedLoop/tests/pyfrc_test.py +++ b/python/PositionClosedLoop/tests/pyfrc_test.py @@ -5,7 +5,7 @@ from time import sleep from pyfrc.tests import * -from phoenix6 import TalonFX, TalonFXConfiguration, PositionVoltage +from phoenix6 import hardware, configs, controls from phoenix6.unmanaged import feed_enable from wpilib.simulation import DCMotorSim from wpimath.system.plant import DCMotor @@ -22,7 +22,7 @@ def assert_almost_equal(a: float, b: float, range_val: float): # PID loop means we should be kinda fast, let's target 10ms LOOP_PERIOD = 0.01 -def wait_with_sim(time: float, fx: TalonFX, dcmotorsim: DCMotorSim): +def wait_with_sim(time: float, fx: hardware.TalonFX, dcmotorsim: DCMotorSim): start_time = 0 while start_time < time: feed_enable(LOOP_PERIOD * 2) @@ -36,14 +36,14 @@ def wait_with_sim(time: float, fx: TalonFX, dcmotorsim: DCMotorSim): sleep(LOOP_PERIOD) def test_position_closed_loop(): - talonfx = TalonFX(1, "sim") + talonfx = hardware.TalonFX(1, "sim") motorsim = DCMotorSim(DCMotor.falcon500FOC(1), 1.0, 0.001) pos = talonfx.get_position() talonfx.sim_state.set_raw_rotor_position(radiansToRotations(motorsim.getAngularPosition())) talonfx.sim_state.set_supply_voltage(12) - cfg = TalonFXConfiguration() + cfg = configs.TalonFXConfiguration() cfg.slot0.k_p = 2 cfg.slot0.k_d = 0.1 assert talonfx.configurator.apply(cfg).is_ok() @@ -53,7 +53,7 @@ def test_position_closed_loop(): assert_almost_equal(pos.value, 0, 0.01) # Closed loop for 1 seconds to a target of 1 rotation, and verify we're close - target_control = PositionVoltage(position=1.0) + target_control = controls.PositionVoltage(position=1.0) talonfx.set_control(target_control) wait_with_sim(1, talonfx, motorsim) diff --git a/python/StatusSignals/robot.py b/python/StatusSignals/robot.py index f88a99a1..78dfcb57 100644 --- a/python/StatusSignals/robot.py +++ b/python/StatusSignals/robot.py @@ -5,8 +5,8 @@ import wpilib from wpilib import Timer, XboxController from phoenix6 import ( - TalonFX, - DutyCycleOut, + hardware, + controls, SignalLogger, BaseStatusSignal, ) @@ -22,8 +22,8 @@ def robotInit(self): """Robot initialization function""" # Keep a reference to all the motor controllers used - self.motor = TalonFX(1, "canivore") - self.request = DutyCycleOut(0) + self.motor = hardware.TalonFX(1, "canivore") + self.request = controls.DutyCycleOut(0) self.pos = self.motor.get_position() self.vel = self.motor.get_velocity() diff --git a/python/StatusSignals/tests/pyfrc_test.py b/python/StatusSignals/tests/pyfrc_test.py index c5392747..19901404 100644 --- a/python/StatusSignals/tests/pyfrc_test.py +++ b/python/StatusSignals/tests/pyfrc_test.py @@ -5,7 +5,7 @@ from time import sleep from pyfrc.tests import * -from phoenix6 import TalonFX, BaseStatusSignal +from phoenix6 import hardware, BaseStatusSignal POS = 1.5 VEL = 10 @@ -21,7 +21,7 @@ def test_latency_compensation(): """ Verifies that latency compensation is performing as expected """ - fx = TalonFX(1, "sim") + fx = hardware.TalonFX(1, "sim") pos = fx.get_position() vel = fx.get_velocity() diff --git a/python/TalonFX/robot.py b/python/TalonFX/robot.py index 5bc846fe..e276cfcf 100644 --- a/python/TalonFX/robot.py +++ b/python/TalonFX/robot.py @@ -4,7 +4,7 @@ """ import wpilib from wpilib import Timer -from phoenix6 import TalonFX +from phoenix6 import hardware class MyRobot(wpilib.TimedRobot): """ @@ -16,7 +16,7 @@ def robotInit(self): """Robot initialization function""" # Keep a reference to all the motor controllers used - self.talonfx = TalonFX(1, "canivore") + self.talonfx = hardware.TalonFX(1, "canivore") self.timer = Timer() self.timer.start() diff --git a/python/TalonFX/tests/pyfrc_test.py b/python/TalonFX/tests/pyfrc_test.py index b8adee14..6d0a10d9 100644 --- a/python/TalonFX/tests/pyfrc_test.py +++ b/python/TalonFX/tests/pyfrc_test.py @@ -4,7 +4,7 @@ ''' from pyfrc.tests import * -from phoenix6 import TalonFX, TalonFXConfiguration +from phoenix6 import hardware, configs FIRST_SET = 0 SECOND_SET = 4.8 @@ -16,10 +16,10 @@ def assert_almost_equal(a: float, b: float, range_val: float): assert a >= (b - range_val) and a <= (b + range_val) def test_pos_setter(): - talonfx = talonfx(1, "sim") + talonfx = hardware.TalonFX(1, "sim") pos = talonfx.get_position() - cfg = TalonFXConfiguration() + cfg = configs.TalonFXConfiguration() talonfx.configurator.apply(cfg) talonfx.set_position(FIRST_SET) From 1299ff6a20526a2ab6cdc3a65fe4b3602b5a3505 Mon Sep 17 00:00:00 2001 From: CoryNessCTR Date: Mon, 18 Dec 2023 14:07:46 -0500 Subject: [PATCH 12/16] Remove .deploy_cfg & .installer_config, add to gitignore --- python/ArcadeDrive/.deploy_cfg | 3 --- python/ArcadeDrive/.gitignore | 2 ++ python/ArcadeDrive/.installer_config | 3 --- python/CANcoder/.deploy_cfg | 3 --- python/CANcoder/.gitignore | 2 ++ python/CANcoder/.installer_config | 3 --- python/PositionClosedLoop/.deploy_cfg | 3 --- python/PositionClosedLoop/.gitignore | 2 ++ python/PositionClosedLoop/.installer_config | 3 --- python/StatusSignals/.deploy_cfg | 3 --- python/StatusSignals/.gitignore | 2 ++ python/StatusSignals/.installer_config | 3 --- python/TalonFX/.deploy_cfg | 3 --- python/TalonFX/.gitignore | 2 ++ python/TalonFX/.installer_config | 3 --- 15 files changed, 10 insertions(+), 30 deletions(-) delete mode 100644 python/ArcadeDrive/.deploy_cfg delete mode 100644 python/ArcadeDrive/.installer_config delete mode 100644 python/CANcoder/.deploy_cfg delete mode 100644 python/CANcoder/.installer_config delete mode 100644 python/PositionClosedLoop/.deploy_cfg delete mode 100644 python/PositionClosedLoop/.installer_config delete mode 100644 python/StatusSignals/.deploy_cfg delete mode 100644 python/StatusSignals/.installer_config delete mode 100644 python/TalonFX/.deploy_cfg delete mode 100644 python/TalonFX/.installer_config diff --git a/python/ArcadeDrive/.deploy_cfg b/python/ArcadeDrive/.deploy_cfg deleted file mode 100644 index e6134433..00000000 --- a/python/ArcadeDrive/.deploy_cfg +++ /dev/null @@ -1,3 +0,0 @@ -[auth] -hostname = 7762 - diff --git a/python/ArcadeDrive/.gitignore b/python/ArcadeDrive/.gitignore index 692d33ac..d18c5608 100644 --- a/python/ArcadeDrive/.gitignore +++ b/python/ArcadeDrive/.gitignore @@ -1,3 +1,5 @@ ctre_sim __pycache__ .pytest_cache +.deploy_cfg +.installer_config diff --git a/python/ArcadeDrive/.installer_config b/python/ArcadeDrive/.installer_config deleted file mode 100644 index e6134433..00000000 --- a/python/ArcadeDrive/.installer_config +++ /dev/null @@ -1,3 +0,0 @@ -[auth] -hostname = 7762 - diff --git a/python/CANcoder/.deploy_cfg b/python/CANcoder/.deploy_cfg deleted file mode 100644 index e6134433..00000000 --- a/python/CANcoder/.deploy_cfg +++ /dev/null @@ -1,3 +0,0 @@ -[auth] -hostname = 7762 - diff --git a/python/CANcoder/.gitignore b/python/CANcoder/.gitignore index 692d33ac..d18c5608 100644 --- a/python/CANcoder/.gitignore +++ b/python/CANcoder/.gitignore @@ -1,3 +1,5 @@ ctre_sim __pycache__ .pytest_cache +.deploy_cfg +.installer_config diff --git a/python/CANcoder/.installer_config b/python/CANcoder/.installer_config deleted file mode 100644 index e6134433..00000000 --- a/python/CANcoder/.installer_config +++ /dev/null @@ -1,3 +0,0 @@ -[auth] -hostname = 7762 - diff --git a/python/PositionClosedLoop/.deploy_cfg b/python/PositionClosedLoop/.deploy_cfg deleted file mode 100644 index e6134433..00000000 --- a/python/PositionClosedLoop/.deploy_cfg +++ /dev/null @@ -1,3 +0,0 @@ -[auth] -hostname = 7762 - diff --git a/python/PositionClosedLoop/.gitignore b/python/PositionClosedLoop/.gitignore index 692d33ac..d18c5608 100644 --- a/python/PositionClosedLoop/.gitignore +++ b/python/PositionClosedLoop/.gitignore @@ -1,3 +1,5 @@ ctre_sim __pycache__ .pytest_cache +.deploy_cfg +.installer_config diff --git a/python/PositionClosedLoop/.installer_config b/python/PositionClosedLoop/.installer_config deleted file mode 100644 index e6134433..00000000 --- a/python/PositionClosedLoop/.installer_config +++ /dev/null @@ -1,3 +0,0 @@ -[auth] -hostname = 7762 - diff --git a/python/StatusSignals/.deploy_cfg b/python/StatusSignals/.deploy_cfg deleted file mode 100644 index e6134433..00000000 --- a/python/StatusSignals/.deploy_cfg +++ /dev/null @@ -1,3 +0,0 @@ -[auth] -hostname = 7762 - diff --git a/python/StatusSignals/.gitignore b/python/StatusSignals/.gitignore index 692d33ac..d18c5608 100644 --- a/python/StatusSignals/.gitignore +++ b/python/StatusSignals/.gitignore @@ -1,3 +1,5 @@ ctre_sim __pycache__ .pytest_cache +.deploy_cfg +.installer_config diff --git a/python/StatusSignals/.installer_config b/python/StatusSignals/.installer_config deleted file mode 100644 index e6134433..00000000 --- a/python/StatusSignals/.installer_config +++ /dev/null @@ -1,3 +0,0 @@ -[auth] -hostname = 7762 - diff --git a/python/TalonFX/.deploy_cfg b/python/TalonFX/.deploy_cfg deleted file mode 100644 index e6134433..00000000 --- a/python/TalonFX/.deploy_cfg +++ /dev/null @@ -1,3 +0,0 @@ -[auth] -hostname = 7762 - diff --git a/python/TalonFX/.gitignore b/python/TalonFX/.gitignore index 692d33ac..d18c5608 100644 --- a/python/TalonFX/.gitignore +++ b/python/TalonFX/.gitignore @@ -1,3 +1,5 @@ ctre_sim __pycache__ .pytest_cache +.deploy_cfg +.installer_config diff --git a/python/TalonFX/.installer_config b/python/TalonFX/.installer_config deleted file mode 100644 index e6134433..00000000 --- a/python/TalonFX/.installer_config +++ /dev/null @@ -1,3 +0,0 @@ -[auth] -hostname = 7762 - From 880ee5c7cdb9ad6e522f28594ecb2daed4a265b1 Mon Sep 17 00:00:00 2001 From: CoryNessCTR Date: Mon, 18 Dec 2023 14:21:08 -0500 Subject: [PATCH 13/16] Add feed_enable to simulation periodic --- python/ArcadeDrive/robot.py | 8 +++++++- python/CANcoder/robot.py | 9 ++++++++- python/PositionClosedLoop/robot.py | 9 ++++++++- python/StatusSignals/robot.py | 8 ++++++++ python/TalonFX/robot.py | 16 ++++++++++++++-- 5 files changed, 45 insertions(+), 5 deletions(-) diff --git a/python/ArcadeDrive/robot.py b/python/ArcadeDrive/robot.py index d44ffd22..c9dd9c13 100644 --- a/python/ArcadeDrive/robot.py +++ b/python/ArcadeDrive/robot.py @@ -3,7 +3,7 @@ This is a demo program for arcade drive in Python with Phoenix 6 """ import wpilib -from phoenix6 import hardware, controls +from phoenix6 import hardware, controls, unmanaged class MyRobot(wpilib.TimedRobot): @@ -46,6 +46,12 @@ def teleopPeriodic(self): self.front_left_motor.set_control(self.left_out.with_output(throttle + wheel)) self.front_right_motor.set_control(self.right_out.with_output(throttle - wheel)) + def _simulationPeriodic(self): + """""" + # If the driver station is enabled, then feed enable for phoenix devices + if wpilib.DriverStation.isEnabled(): + unmanaged.feed_enable(100) + if __name__ == "__main__": wpilib.run(MyRobot) diff --git a/python/CANcoder/robot.py b/python/CANcoder/robot.py index 988068ec..03f2605d 100644 --- a/python/CANcoder/robot.py +++ b/python/CANcoder/robot.py @@ -4,7 +4,7 @@ """ import wpilib from wpilib import Timer -from phoenix6 import hardware +from phoenix6 import hardware, unmanaged class MyRobot(wpilib.TimedRobot): """ @@ -25,6 +25,7 @@ def teleopPeriodic(self): """Every 100ms, print the status of the StatusSignal""" if self.timer.hasElapsed(0.1): + self.timer.reset() # get_position automatically calls refresh(), no need to manually refresh. # # StatusSignals also implement the str dunder to provide a useful print of the signal @@ -39,5 +40,11 @@ def teleopPeriodic(self): print("") + def _simulationPeriodic(self): + """""" + # If the driver station is enabled, then feed enable for phoenix devices + if wpilib.DriverStation.isEnabled(): + unmanaged.feed_enable(100) + if __name__ == "__main__": wpilib.run(MyRobot) diff --git a/python/PositionClosedLoop/robot.py b/python/PositionClosedLoop/robot.py index 2bc38b16..81312351 100644 --- a/python/PositionClosedLoop/robot.py +++ b/python/PositionClosedLoop/robot.py @@ -4,7 +4,7 @@ """ import wpilib from wpilib import Timer, XboxController -from phoenix6 import hardware, controls, configs +from phoenix6 import hardware, controls, configs, unmanaged class MyRobot(wpilib.TimedRobot): """ @@ -42,6 +42,7 @@ def teleopPeriodic(self): self.talonfx.set_control(self.position_request.with_position(self.joystick.getLeftY() * 10)) if self.timer.hasElapsed(0.1): + self.timer.reset() # Print the position & velocity to see what they are pos = self.talonfx.get_position() vel = self.talonfx.get_velocity() @@ -51,5 +52,11 @@ def teleopPeriodic(self): print("") + def _simulationPeriodic(self): + """""" + # If the driver station is enabled, then feed enable for phoenix devices + if wpilib.DriverStation.isEnabled(): + unmanaged.feed_enable(100) + if __name__ == "__main__": wpilib.run(MyRobot) diff --git a/python/StatusSignals/robot.py b/python/StatusSignals/robot.py index 78dfcb57..cf67fff4 100644 --- a/python/StatusSignals/robot.py +++ b/python/StatusSignals/robot.py @@ -9,6 +9,7 @@ controls, SignalLogger, BaseStatusSignal, + unmanaged ) @@ -47,6 +48,7 @@ def teleopPeriodic(self): """Every 100ms, print the status of the StatusSignal""" if self.timer.hasElapsed(0.1): + self.timer.reset() BaseStatusSignal.refresh_all(self.pos, self.vel) pos_timestamp = self.pos.all_timestamps.get_device_timestamp().time @@ -57,6 +59,12 @@ def teleopPeriodic(self): ) print(f"Latency compensated position is {latency_compensated_pos}") + def _simulationPeriodic(self): + """""" + # If the driver station is enabled, then feed enable for phoenix devices + if wpilib.DriverStation.isEnabled(): + unmanaged.feed_enable(100) + if __name__ == "__main__": wpilib.run(MyRobot) diff --git a/python/TalonFX/robot.py b/python/TalonFX/robot.py index e276cfcf..6282d411 100644 --- a/python/TalonFX/robot.py +++ b/python/TalonFX/robot.py @@ -3,8 +3,8 @@ This is a demo program for TalonFX usage in Phoenix 6 """ import wpilib -from wpilib import Timer -from phoenix6 import hardware +from wpilib import Timer, XboxController +from phoenix6 import hardware, unmanaged, controls class MyRobot(wpilib.TimedRobot): """ @@ -17,14 +17,20 @@ def robotInit(self): # Keep a reference to all the motor controllers used self.talonfx = hardware.TalonFX(1, "canivore") + self.control = controls.DutyCycleOut(0) self.timer = Timer() self.timer.start() + self.joystick = XboxController(0) + def teleopPeriodic(self): """Every 100ms, print the status of the StatusSignal""" + self.talonfx.set_control(self.control.with_output(self.joystick.getLeftY())) + if self.timer.hasElapsed(0.1): + self.timer.reset() # get_position automatically calls refresh(), no need to manually refresh. # # StatusSignals also implement the str dunder to provide a useful print of the signal @@ -39,5 +45,11 @@ def teleopPeriodic(self): print("") + def _simulationPeriodic(self): + """""" + # If the driver station is enabled, then feed enable for phoenix devices + if wpilib.DriverStation.isEnabled(): + unmanaged.feed_enable(100) + if __name__ == "__main__": wpilib.run(MyRobot) From 8df29ba007526c675853dd32c63e9130e3e2cf70 Mon Sep 17 00:00:00 2001 From: CoryNessCTR Date: Tue, 2 Jan 2024 13:54:45 -0500 Subject: [PATCH 14/16] Begin work on physics.py --- python/TalonFX/physics.py | 28 ++++++++++++++++++++++++++++ python/TalonFX/robot.py | 7 +------ 2 files changed, 29 insertions(+), 6 deletions(-) create mode 100644 python/TalonFX/physics.py diff --git a/python/TalonFX/physics.py b/python/TalonFX/physics.py new file mode 100644 index 00000000..e4255cd1 --- /dev/null +++ b/python/TalonFX/physics.py @@ -0,0 +1,28 @@ +import wpilib.simulation + +from pyfrc.physics.core import PhysicsInterface +from phoenix6 import unmanaged + +import typing + +if typing.TYPE_CHECKING: + from robot import MyRobot + + +class PhysicsEngine: + + def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): + pass + + def update_sim(self, now: float, tm_diff: float) -> None: + """ + Called when the simulation parameters for the program need to be + updated. + + :param now: The current time as a float + :param tm_diff: The amount of time that has passed since the last + time that this function was called + """ + # If the driver station is enabled, then feed enable for phoenix devices + if wpilib.DriverStation.isEnabled(): + unmanaged.feed_enable(100) \ No newline at end of file diff --git a/python/TalonFX/robot.py b/python/TalonFX/robot.py index 6282d411..701bf7c3 100644 --- a/python/TalonFX/robot.py +++ b/python/TalonFX/robot.py @@ -4,7 +4,7 @@ """ import wpilib from wpilib import Timer, XboxController -from phoenix6 import hardware, unmanaged, controls +from phoenix6 import hardware, controls class MyRobot(wpilib.TimedRobot): """ @@ -45,11 +45,6 @@ def teleopPeriodic(self): print("") - def _simulationPeriodic(self): - """""" - # If the driver station is enabled, then feed enable for phoenix devices - if wpilib.DriverStation.isEnabled(): - unmanaged.feed_enable(100) if __name__ == "__main__": wpilib.run(MyRobot) From 5cdab895b34eaafcd9c75c16928fca7a8f586bce Mon Sep 17 00:00:00 2001 From: CoryNessCTR Date: Mon, 12 Feb 2024 15:16:39 -0500 Subject: [PATCH 15/16] Further improvements to simulation --- python/ArcadeDrive/physics.py | 74 ++++++++++++++++++++++++++++ python/ArcadeDrive/robot.py | 16 +++--- python/CANcoder/physics.py | 46 +++++++++++++++++ python/CANcoder/robot.py | 9 +--- python/PositionClosedLoop/physics.py | 43 ++++++++++++++++ python/PositionClosedLoop/robot.py | 8 +-- python/StatusSignals/physics.py | 43 ++++++++++++++++ python/StatusSignals/robot.py | 6 --- python/TalonFX/physics.py | 23 +++++++-- 9 files changed, 236 insertions(+), 32 deletions(-) create mode 100644 python/ArcadeDrive/physics.py create mode 100644 python/CANcoder/physics.py create mode 100644 python/PositionClosedLoop/physics.py create mode 100644 python/StatusSignals/physics.py diff --git a/python/ArcadeDrive/physics.py b/python/ArcadeDrive/physics.py new file mode 100644 index 00000000..b7fc4e91 --- /dev/null +++ b/python/ArcadeDrive/physics.py @@ -0,0 +1,74 @@ +import wpilib.simulation as sim +from wpilib import RobotController, DriverStation + +from wpimath.system.plant import DCMotor +from wpimath.units import radiansToRotations + +from pyfrc.physics.core import PhysicsInterface +from pyfrc.physics import motor_cfgs, tankmodel +from pyfrc.physics.units import units + +from phoenix6 import unmanaged + +import math + +import typing + +if typing.TYPE_CHECKING: + from robot import MyRobot + + +class PhysicsEngine: + + def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): + self.physics_controller = physics_controller + + # Keep a reference to the motor sim state so we can update it + self.left_talon_sim = robot.front_left_motor.sim_state + self.right_talon_sim = robot.front_right_motor.sim_state + + # Change these parameters to fit your robot! + bumper_width = 3.25 * units.inch + + self.wheel_radius = 3 + self.drivetrain = tankmodel.TankModel.theory( + motor_cfgs.MOTOR_CFG_FALCON_500, # motor configuration + 110 * units.lbs, # robot mass + 10.71, # drivetrain gear ratio + 2, # motors per side + 22 * units.inch, # robot wheelbase + 23 * units.inch + bumper_width * 2, # robot width + 32 * units.inch + bumper_width * 2, # robot length + self.wheel_radius * 2 * units.inch, # wheel diameter + ) + + def update_sim(self, now: float, tm_diff: float) -> None: + """ + Called when the simulation parameters for the program need to be + updated. + + :param now: The current time as a float + :param tm_diff: The amount of time that has passed since the last + time that this function was called + """ + # If the driver station is enabled, then feed enable for phoenix devices + if DriverStation.isEnabled(): + unmanaged.feed_enable(100) + + battery_v = RobotController.getBatteryVoltage() + self.left_talon_sim.set_supply_voltage(battery_v) + self.right_talon_sim.set_supply_voltage(battery_v) + + transform = self.drivetrain.calculate(self.left_talon_sim.motor_voltage / battery_v, self.right_talon_sim.motor_voltage / battery_v, tm_diff) + self.physics_controller.move_robot(transform) + + self.left_talon_sim.set_raw_rotor_position(self.feet_to_rotations(self.drivetrain.l_position)) + self.left_talon_sim.set_rotor_velocity(self.feet_to_rotations(self.drivetrain.l_velocity)) + self.right_talon_sim.set_raw_rotor_position(self.feet_to_rotations(self.drivetrain.r_position)) + self.right_talon_sim.set_rotor_velocity(self.feet_to_rotations(self.drivetrain.r_velocity)) + + def feet_to_rotations(self, dist: float) -> float: + circumference = self.wheel_radius * 2.0 * math.pi + rotations_per_foot = 1.0 / (circumference / 12.0) + return rotations_per_foot * dist + diff --git a/python/ArcadeDrive/robot.py b/python/ArcadeDrive/robot.py index c9dd9c13..f3cef4cd 100644 --- a/python/ArcadeDrive/robot.py +++ b/python/ArcadeDrive/robot.py @@ -3,7 +3,7 @@ This is a demo program for arcade drive in Python with Phoenix 6 """ import wpilib -from phoenix6 import hardware, controls, unmanaged +from phoenix6 import hardware, controls, configs class MyRobot(wpilib.TimedRobot): @@ -22,6 +22,13 @@ def robotInit(self): self.front_right_motor = hardware.TalonFX(2, canivore_name) self.rear_right_motor = hardware.TalonFX(3, canivore_name) + cfg = configs.TalonFXConfiguration() + cfg.motor_output.inverted = configs.config_groups.InvertedValue.COUNTER_CLOCKWISE_POSITIVE + self.front_left_motor.configurator.apply(cfg) + + cfg.motor_output.inverted = configs.config_groups.InvertedValue.CLOCKWISE_POSITIVE + self.front_right_motor.configurator.apply(cfg) + # Configure the rear motors to follow the front motors follow_left_request = controls.Follower(0, False) self.rear_left_motor.set_control(follow_left_request) @@ -46,12 +53,5 @@ def teleopPeriodic(self): self.front_left_motor.set_control(self.left_out.with_output(throttle + wheel)) self.front_right_motor.set_control(self.right_out.with_output(throttle - wheel)) - def _simulationPeriodic(self): - """""" - # If the driver station is enabled, then feed enable for phoenix devices - if wpilib.DriverStation.isEnabled(): - unmanaged.feed_enable(100) - - if __name__ == "__main__": wpilib.run(MyRobot) diff --git a/python/CANcoder/physics.py b/python/CANcoder/physics.py new file mode 100644 index 00000000..dca307b4 --- /dev/null +++ b/python/CANcoder/physics.py @@ -0,0 +1,46 @@ +import wpilib.simulation as sim +from wpilib import RobotController, DriverStation + +from wpimath.system.plant import DCMotor +from wpimath.units import radiansToRotations + +from pyfrc.physics.core import PhysicsInterface +from phoenix6 import unmanaged + +import typing + +if typing.TYPE_CHECKING: + from robot import MyRobot + + +class PhysicsEngine: + + def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): + self.physics_controller = physics_controller + + # Create a DCMotorSim for physics sim + self.motor_sim = sim.DCMotorSim(DCMotor.krakenX60FOC(1), 1, 0.01) + # Keep a reference to the cancoder sim state so we can update it + self.cancoder_sim = robot.cancoder.sim_state + + # Keep a reference to the controller so we can drive a simulated motor + self.controller = robot.controller + + def update_sim(self, now: float, tm_diff: float) -> None: + """ + Called when the simulation parameters for the program need to be + updated. + + :param now: The current time as a float + :param tm_diff: The amount of time that has passed since the last + time that this function was called + """ + # If the driver station is enabled, then feed enable for phoenix devices + if DriverStation.isEnabled(): + unmanaged.feed_enable(100) + + self.cancoder_sim.set_supply_voltage(RobotController.getBatteryVoltage()) + self.motor_sim.setInputVoltage(self.controller.getLeftY() * 12) + self.motor_sim.update(tm_diff) + self.cancoder_sim.set_raw_position(radiansToRotations(self.motor_sim.getAngularPosition())) + self.cancoder_sim.set_velocity(radiansToRotations(self.motor_sim.getAngularVelocity())) diff --git a/python/CANcoder/robot.py b/python/CANcoder/robot.py index 03f2605d..d0b69f7d 100644 --- a/python/CANcoder/robot.py +++ b/python/CANcoder/robot.py @@ -4,7 +4,7 @@ """ import wpilib from wpilib import Timer -from phoenix6 import hardware, unmanaged +from phoenix6 import hardware class MyRobot(wpilib.TimedRobot): """ @@ -20,6 +20,7 @@ def robotInit(self): self.timer = Timer() self.timer.start() + self.controller = wpilib.XboxController(0) def teleopPeriodic(self): """Every 100ms, print the status of the StatusSignal""" @@ -40,11 +41,5 @@ def teleopPeriodic(self): print("") - def _simulationPeriodic(self): - """""" - # If the driver station is enabled, then feed enable for phoenix devices - if wpilib.DriverStation.isEnabled(): - unmanaged.feed_enable(100) - if __name__ == "__main__": wpilib.run(MyRobot) diff --git a/python/PositionClosedLoop/physics.py b/python/PositionClosedLoop/physics.py new file mode 100644 index 00000000..2c08b563 --- /dev/null +++ b/python/PositionClosedLoop/physics.py @@ -0,0 +1,43 @@ +import wpilib.simulation as sim +from wpilib import RobotController, DriverStation + +from wpimath.system.plant import DCMotor +from wpimath.units import radiansToRotations + +from pyfrc.physics.core import PhysicsInterface +from phoenix6 import unmanaged + +import typing + +if typing.TYPE_CHECKING: + from robot import MyRobot + + +class PhysicsEngine: + + def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): + self.physics_controller = physics_controller + + # Create a DCMotorSim for physics sim + self.motor_sim = sim.DCMotorSim(DCMotor.krakenX60FOC(1), 1, 0.01) + # Keep a reference to the motor sim state so we can update it + self.talon_sim = robot.talonfx.sim_state + + def update_sim(self, now: float, tm_diff: float) -> None: + """ + Called when the simulation parameters for the program need to be + updated. + + :param now: The current time as a float + :param tm_diff: The amount of time that has passed since the last + time that this function was called + """ + # If the driver station is enabled, then feed enable for phoenix devices + if DriverStation.isEnabled(): + unmanaged.feed_enable(100) + + self.talon_sim.set_supply_voltage(RobotController.getBatteryVoltage()) + self.motor_sim.setInputVoltage(self.talon_sim.motor_voltage) + self.motor_sim.update(tm_diff) + self.talon_sim.set_raw_rotor_position(radiansToRotations(self.motor_sim.getAngularPosition())) + self.talon_sim.set_rotor_velocity(radiansToRotations(self.motor_sim.getAngularVelocity())) diff --git a/python/PositionClosedLoop/robot.py b/python/PositionClosedLoop/robot.py index 81312351..84d7cd12 100644 --- a/python/PositionClosedLoop/robot.py +++ b/python/PositionClosedLoop/robot.py @@ -4,7 +4,7 @@ """ import wpilib from wpilib import Timer, XboxController -from phoenix6 import hardware, controls, configs, unmanaged +from phoenix6 import hardware, controls, configs class MyRobot(wpilib.TimedRobot): """ @@ -52,11 +52,5 @@ def teleopPeriodic(self): print("") - def _simulationPeriodic(self): - """""" - # If the driver station is enabled, then feed enable for phoenix devices - if wpilib.DriverStation.isEnabled(): - unmanaged.feed_enable(100) - if __name__ == "__main__": wpilib.run(MyRobot) diff --git a/python/StatusSignals/physics.py b/python/StatusSignals/physics.py new file mode 100644 index 00000000..d479ba46 --- /dev/null +++ b/python/StatusSignals/physics.py @@ -0,0 +1,43 @@ +import wpilib.simulation as sim +from wpilib import RobotController, DriverStation + +from wpimath.system.plant import DCMotor +from wpimath.units import radiansToRotations + +from pyfrc.physics.core import PhysicsInterface +from phoenix6 import unmanaged + +import typing + +if typing.TYPE_CHECKING: + from robot import MyRobot + + +class PhysicsEngine: + + def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): + self.physics_controller = physics_controller + + # Create a DCMotorSim for physics sim + self.motor_sim = sim.DCMotorSim(DCMotor.krakenX60FOC(1), 1, 0.01) + # Keep a reference to the motor sim state so we can update it + self.talon_sim = robot.motor.sim_state + + def update_sim(self, now: float, tm_diff: float) -> None: + """ + Called when the simulation parameters for the program need to be + updated. + + :param now: The current time as a float + :param tm_diff: The amount of time that has passed since the last + time that this function was called + """ + # If the driver station is enabled, then feed enable for phoenix devices + if DriverStation.isEnabled(): + unmanaged.feed_enable(100) + + self.talon_sim.set_supply_voltage(RobotController.getBatteryVoltage()) + self.motor_sim.setInputVoltage(self.talon_sim.motor_voltage) + self.motor_sim.update(tm_diff) + self.talon_sim.set_raw_rotor_position(radiansToRotations(self.motor_sim.getAngularPosition())) + self.talon_sim.set_rotor_velocity(radiansToRotations(self.motor_sim.getAngularVelocity())) diff --git a/python/StatusSignals/robot.py b/python/StatusSignals/robot.py index cf67fff4..9c4ed479 100644 --- a/python/StatusSignals/robot.py +++ b/python/StatusSignals/robot.py @@ -59,12 +59,6 @@ def teleopPeriodic(self): ) print(f"Latency compensated position is {latency_compensated_pos}") - def _simulationPeriodic(self): - """""" - # If the driver station is enabled, then feed enable for phoenix devices - if wpilib.DriverStation.isEnabled(): - unmanaged.feed_enable(100) - if __name__ == "__main__": wpilib.run(MyRobot) diff --git a/python/TalonFX/physics.py b/python/TalonFX/physics.py index e4255cd1..2c08b563 100644 --- a/python/TalonFX/physics.py +++ b/python/TalonFX/physics.py @@ -1,4 +1,8 @@ -import wpilib.simulation +import wpilib.simulation as sim +from wpilib import RobotController, DriverStation + +from wpimath.system.plant import DCMotor +from wpimath.units import radiansToRotations from pyfrc.physics.core import PhysicsInterface from phoenix6 import unmanaged @@ -12,7 +16,12 @@ class PhysicsEngine: def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): - pass + self.physics_controller = physics_controller + + # Create a DCMotorSim for physics sim + self.motor_sim = sim.DCMotorSim(DCMotor.krakenX60FOC(1), 1, 0.01) + # Keep a reference to the motor sim state so we can update it + self.talon_sim = robot.talonfx.sim_state def update_sim(self, now: float, tm_diff: float) -> None: """ @@ -24,5 +33,11 @@ def update_sim(self, now: float, tm_diff: float) -> None: time that this function was called """ # If the driver station is enabled, then feed enable for phoenix devices - if wpilib.DriverStation.isEnabled(): - unmanaged.feed_enable(100) \ No newline at end of file + if DriverStation.isEnabled(): + unmanaged.feed_enable(100) + + self.talon_sim.set_supply_voltage(RobotController.getBatteryVoltage()) + self.motor_sim.setInputVoltage(self.talon_sim.motor_voltage) + self.motor_sim.update(tm_diff) + self.talon_sim.set_raw_rotor_position(radiansToRotations(self.motor_sim.getAngularPosition())) + self.talon_sim.set_rotor_velocity(radiansToRotations(self.motor_sim.getAngularVelocity())) From a9e95050104539d88b65abf7d459692d434b9dd7 Mon Sep 17 00:00:00 2001 From: CoryNessCTR Date: Mon, 12 Feb 2024 15:23:51 -0500 Subject: [PATCH 16/16] Update CI for new examples & new method of executing robotpy projects --- .github/workflows/build-all-parallel.yml | 6 ++++-- build_parallel_workflow.py | 4 ++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build-all-parallel.yml b/.github/workflows/build-all-parallel.yml index c4495bdb..695935f6 100644 --- a/.github/workflows/build-all-parallel.yml +++ b/.github/workflows/build-all-parallel.yml @@ -61,6 +61,8 @@ jobs: directory: 'java/FusedCANcoder' - project-name: 'MotionMagic' directory: 'java/MotionMagic' + - project-name: 'PhoenixSysId' + directory: 'java/PhoenixSysId' - project-name: 'Pigeon2' directory: 'java/Pigeon2' - project-name: 'PositionClosedLoop' @@ -98,7 +100,7 @@ jobs: matrix: python_version: ['3.9', '3.10', '3.11'] os: ['ubuntu-22.04', 'macos-12', 'windows-2022'] - project-name: ['ArcadeDrive', 'CANcoder', 'StatusSignals'] + project-name: ['ArcadeDrive', 'CANcoder', 'PositionClosedLoop', 'StatusSignals', 'TalonFX'] runs-on: ${{ matrix.os }} @@ -113,4 +115,4 @@ jobs: pip install 'robotpy' phoenix6 - name: Test ${{ matrix.project-name }} run: | - cd "python/${{ matrix.project-name }}" && python3 robot.py test + cd "python/${{ matrix.project-name }}" && python3 -m robotpy test diff --git a/build_parallel_workflow.py b/build_parallel_workflow.py index 62e88865..c7bae9e6 100644 --- a/build_parallel_workflow.py +++ b/build_parallel_workflow.py @@ -63,7 +63,7 @@ pip install 'robotpy' phoenix6 - name: Test ${{{{ matrix.project-name }}}} run: | - cd "python/${{{{ matrix.project-name }}}}" && python3 robot.py test + cd "python/${{{{ matrix.project-name }}}}" && python3 -m robotpy test """ PROJECT_MATRIX_TEMPLATE = """ @@ -81,7 +81,7 @@ # Find every project in here and build up an array of strings to generate the workflow file for project in os.listdir(project_dir): project_matrix.append(PROJECT_MATRIX_TEMPLATE.format(project_name=project, project_dir=f"{project_dir}/{project}")) - + python_project_matrix = [] for project_dir in PYTHON_PROJECTS_TO_SEARCH: # Find every project in here and build up an array of strings to generate the workflow file