Skip to content

Commit

Permalink
Fix tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@bhall-ctre
bhall-ctre committed May 29, 2024
1 parent 79b30ce commit 1d96843
Show file tree
Hide file tree
Showing 3 changed files with 88 additions and 70 deletions.
60 changes: 33 additions & 27 deletions python/MotionMagic/tests/pyfrc_test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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)
Expand All @@ -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)
48 changes: 27 additions & 21 deletions python/PositionClosedLoop/tests/pyfrc_test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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)
Expand All @@ -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)
50 changes: 28 additions & 22 deletions python/VelocityClosedLoop/tests/pyfrc_test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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)
Expand All @@ -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)

0 comments on commit 1d96843

Please sign in to comment.