From 1d96843319cc47d7caf2eb601682f0748cdf0b36 Mon Sep 17 00:00:00 2001 From: Benjamin Hall Date: Wed, 29 May 2024 14:52:55 -0400 Subject: [PATCH] Fix tests --- python/MotionMagic/tests/pyfrc_test.py | 60 ++++++++++--------- python/PositionClosedLoop/tests/pyfrc_test.py | 48 ++++++++------- python/VelocityClosedLoop/tests/pyfrc_test.py | 50 +++++++++------- 3 files changed, 88 insertions(+), 70 deletions(-) diff --git a/python/MotionMagic/tests/pyfrc_test.py b/python/MotionMagic/tests/pyfrc_test.py index 1c813d28..9a6c9757 100644 --- a/python/MotionMagic/tests/pyfrc_test.py +++ b/python/MotionMagic/tests/pyfrc_test.py @@ -4,6 +4,8 @@ ''' from time import sleep +from robot import MyRobot + from pyfrc.tests import * from phoenix6 import hardware, configs, controls from phoenix6.unmanaged import feed_enable @@ -23,6 +25,9 @@ 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: hardware.TalonFX, dcmotorsim: DCMotorSim): + feed_enable(0.2) + sleep(0.1) + start_time = 0 while start_time < time: feed_enable(0.1) @@ -35,38 +40,39 @@ def wait_with_sim(time: float, fx: hardware.TalonFX, dcmotorsim: DCMotorSim): sleep(LOOP_PERIOD) -def test_position_closed_loop(): - talonfx = hardware.TalonFX(1, "sim") - motorsim = DCMotorSim(DCMotor.falcon500FOC(1), 1.0, 0.001) - pos = talonfx.get_position() +def test_position_closed_loop(control, robot: MyRobot): + with control.run_robot(): + talonfx = robot.talonfx + motorsim = DCMotorSim(DCMotor.krakenX60FOC(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) + talonfx.sim_state.set_raw_rotor_position(radiansToRotations(motorsim.getAngularPosition())) + talonfx.sim_state.set_supply_voltage(12) - cfg = configs.TalonFXConfiguration() - cfg.feedback.sensor_to_mechanism_ratio = 12.8 - cfg.motion_magic.motion_magic_cruise_velocity = 5 - cfg.motion_magic.motion_magic_acceleration = 10 - cfg.motion_magic.motion_magic_jerk = 100 + cfg = configs.TalonFXConfiguration() + cfg.feedback.sensor_to_mechanism_ratio = 12.8 + cfg.motion_magic.motion_magic_cruise_velocity = 5 + cfg.motion_magic.motion_magic_acceleration = 10 + cfg.motion_magic.motion_magic_jerk = 100 - cfg.slot0.k_v = 0.12 - cfg.slot0.k_a = 0.01 - cfg.slot0.k_p = 60 - cfg.slot0.k_i = 0 - cfg.slot0.k_d = 0.5 + cfg.slot0.k_v = 0.12 + cfg.slot0.k_a = 0.01 + cfg.slot0.k_p = 60 + cfg.slot0.k_i = 0 + cfg.slot0.k_d = 0.5 - assert talonfx.configurator.apply(cfg).is_ok() - assert talonfx.set_position(FIRST_SET).is_ok() + 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.02) + pos.wait_for_update(1) + assert_almost_equal(pos.value, 0, 0.02) - # Closed loop for 1 seconds to a target of 1 rotation, and verify we're close - target_control = controls.MotionMagicVoltage(position=1.0) - talonfx.set_control(target_control) + # Closed loop for 1 seconds to a target of 1 rotation, and verify we're close + target_control = controls.MotionMagicVoltage(position=1.0) + talonfx.set_control(target_control) - wait_with_sim(1, talonfx, motorsim) + wait_with_sim(2, talonfx, motorsim) - # Verify position is close to target - pos.wait_for_update(1) - assert_almost_equal(pos.value, 1, 0.02) + # Verify position is close to target + pos.wait_for_update(1) + assert_almost_equal(pos.value, 1, 0.02) diff --git a/python/PositionClosedLoop/tests/pyfrc_test.py b/python/PositionClosedLoop/tests/pyfrc_test.py index 47256591..632a3a60 100644 --- a/python/PositionClosedLoop/tests/pyfrc_test.py +++ b/python/PositionClosedLoop/tests/pyfrc_test.py @@ -4,6 +4,8 @@ ''' from time import sleep +from robot import MyRobot + from pyfrc.tests import * from phoenix6 import hardware, configs, controls from phoenix6.unmanaged import feed_enable @@ -23,6 +25,9 @@ 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: hardware.TalonFX, dcmotorsim: DCMotorSim): + feed_enable(0.2) + sleep(0.1) + start_time = 0 while start_time < time: feed_enable(0.1) @@ -35,30 +40,31 @@ def wait_with_sim(time: float, fx: hardware.TalonFX, dcmotorsim: DCMotorSim): sleep(LOOP_PERIOD) -def test_position_closed_loop(): - talonfx = hardware.TalonFX(1, "sim") - motorsim = DCMotorSim(DCMotor.krakenX60FOC(1), 1.0, 0.001) - pos = talonfx.get_position() +def test_position_closed_loop(control, robot: MyRobot): + with control.run_robot(): + talonfx = robot.talonfx + motorsim = DCMotorSim(DCMotor.krakenX60FOC(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) + talonfx.sim_state.set_raw_rotor_position(radiansToRotations(motorsim.getAngularPosition())) + talonfx.sim_state.set_supply_voltage(12) - cfg = configs.TalonFXConfiguration() - cfg.slot0.k_p = 2.4 - cfg.slot0.k_i = 0 - cfg.slot0.k_d = 0.1 - assert talonfx.configurator.apply(cfg).is_ok() - assert talonfx.set_position(FIRST_SET).is_ok() + cfg = configs.TalonFXConfiguration() + cfg.slot0.k_p = 2.4 + cfg.slot0.k_i = 0 + 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.02) + pos.wait_for_update(1) + assert_almost_equal(pos.value, 0, 0.02) - # Closed loop for 1 seconds to a target of 1 rotation, and verify we're close - target_control = controls.PositionVoltage(position=1.0) - talonfx.set_control(target_control) + # Closed loop for 1 seconds to a target of 1 rotation, and verify we're close + target_control = controls.PositionVoltage(position=1.0) + talonfx.set_control(target_control) - wait_with_sim(1, talonfx, motorsim) + wait_with_sim(2, talonfx, motorsim) - # Verify position is close to target - pos.wait_for_update(1) - assert_almost_equal(pos.value, 1, 0.02) + # Verify position is close to target + pos.wait_for_update(1) + assert_almost_equal(pos.value, 1, 0.02) diff --git a/python/VelocityClosedLoop/tests/pyfrc_test.py b/python/VelocityClosedLoop/tests/pyfrc_test.py index f64d00d4..1aa53daa 100644 --- a/python/VelocityClosedLoop/tests/pyfrc_test.py +++ b/python/VelocityClosedLoop/tests/pyfrc_test.py @@ -4,6 +4,8 @@ ''' from time import sleep +from robot import MyRobot + from pyfrc.tests import * from phoenix6 import hardware, configs, controls from phoenix6.unmanaged import feed_enable @@ -20,6 +22,9 @@ 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: hardware.TalonFX, dcmotorsim: DCMotorSim): + feed_enable(0.2) + sleep(0.1) + start_time = 0 while start_time < time: feed_enable(0.1) @@ -32,31 +37,32 @@ def wait_with_sim(time: float, fx: hardware.TalonFX, dcmotorsim: DCMotorSim): sleep(LOOP_PERIOD) -def test_velocity_closed_loop(): - talonfx = hardware.TalonFX(0, "sim") - motorsim = DCMotorSim(DCMotor.krakenX60FOC(1), 1.0, 0.001) - vel = talonfx.get_velocity() +def test_velocity_closed_loop(control, robot: MyRobot): + with control.run_robot(): + talonfx = robot.talonfx + motorsim = DCMotorSim(DCMotor.krakenX60FOC(1), 1.0, 0.001) + vel = talonfx.get_velocity() - talonfx.sim_state.set_raw_rotor_position(radiansToRotations(motorsim.getAngularPosition())) - talonfx.sim_state.set_rotor_velocity(radiansToRotations(motorsim.getAngularVelocity())) - talonfx.sim_state.set_supply_voltage(12) + talonfx.sim_state.set_raw_rotor_position(radiansToRotations(motorsim.getAngularPosition())) + talonfx.sim_state.set_rotor_velocity(radiansToRotations(motorsim.getAngularVelocity())) + talonfx.sim_state.set_supply_voltage(12) - cfg = configs.TalonFXConfiguration() - cfg.slot0.k_v = 0.12 - cfg.slot0.k_p = 0.11 - cfg.slot0.k_i = 0 - cfg.slot0.k_d = 0 - assert talonfx.configurator.apply(cfg).is_ok() + cfg = configs.TalonFXConfiguration() + cfg.slot0.k_v = 0.12 + cfg.slot0.k_p = 0.11 + cfg.slot0.k_i = 0 + cfg.slot0.k_d = 0 + assert talonfx.configurator.apply(cfg).is_ok() - vel.wait_for_update(1) - assert_almost_equal(vel.value, 0, 0.2) + vel.wait_for_update(1) + assert_almost_equal(vel.value, 0, 0.2) - # Closed loop for 1 second to a target of 10 rps, and verify we're close - target_control = controls.VelocityVoltage(velocity=10) - talonfx.set_control(target_control) + # Closed loop for 1 second to a target of 10 rps, and verify we're close + target_control = controls.VelocityVoltage(velocity=10) + talonfx.set_control(target_control) - wait_with_sim(1, talonfx, motorsim) + wait_with_sim(2, talonfx, motorsim) - # Verify velocity is close to target - vel.wait_for_update(1) - assert_almost_equal(vel.value, 10, 0.2) + # Verify velocity is close to target + vel.wait_for_update(1) + assert_almost_equal(vel.value, 10, 0.2)