def test_hatch_height():
e = Elevator()
setup_tunables(e, "Elevator")
with patch("components.elevator.Elevator.current_height",
new_callable=PropertyMock) as ch:
ch.return_value = 50
assert e.hatch_height == 50 + e.hatch_height_const
def test_next_fn2(wpitime):
class _TM(StateMachine):
@state
def second_state(self):
pass
@timed_state(first=True, duration=0.1, next_state=second_state)
def first_state(self):
pass
sm = _TM()
setup_tunables(sm, 'cname')
sm.engage()
sm.execute()
assert sm.current_state == 'first_state'
assert sm.is_executing
wpitime.now += 0.5
sm.engage()
sm.execute()
assert sm.current_state == 'second_state'
assert sm.is_executing
sm.execute()
assert sm.current_state == ''
assert not sm.is_executing
def test_move():
dt = Drivetrain()
setup_tunables(dt, "Drivetrain")
with patch("components.drivetrain.Drivetrain.current_angle",
new_callable=PropertyMock) as d:
d.return_value = 30
dt.move(1,
0,
0,
driver=False,
slow=False,
gyro=False,
gyro_manually_set=False)
assert (dt.forward == 1 * dt.forward_multiplier
and dt.strafe == 0 * dt.strafe_multiplier
and dt.rotate == 0 * dt.rotate_multiplier)
dt.move(0,
0,
1,
driver=True,
slow=False,
gyro=True,
gyro_manually_set=False)
assert (dt.forward == 0 * dt.forward_multiplier
and dt.strafe == 0 * dt.strafe_multiplier and dt.rotate == 1
and dt.target_angle
== 1 * dt.gyro_rotate_to_angle_multiplier + dt.current_angle)
def test_autonomous_sm():
class _TM(AutonomousStateMachine):
i = 0
VERBOSE_LOGGING = False
@state(first=True)
def something(self):
self.i += 1
if self.i == 6:
self.done()
sm = _TM()
setup_tunables(sm, 'cname')
sm.on_enable()
for _ in range(5):
sm.on_iteration(None)
assert sm.is_executing
sm.on_iteration(None)
assert not sm.is_executing
for _ in range(5):
sm.on_iteration(None)
assert not sm.is_executing
def test_shoot_boulder(control):
ba = BoulderAutomation()
ba.intake = MagicMock()
ba.shooter = MagicMock()
ba.shooter.up_to_speed = MagicMock(return_value=True)
setup_tunables(ba, "boulder_automation")
def _on_step(tm, step):
if step == 1:
ba.shoot_boulder()
assert ba.current_state == "pre_fire"
elif step == 2:
# Requires a step for the test for up_to_speed to return true
assert ba.shooter.shoot.called
elif step <= 20:
assert ba.current_state == "firing"
assert ba.is_executing
assert ba.intake.intake.called
if step > 30: # Should inject ball for around 25 steps - 0.5s
assert not ba.is_executing
assert ba.intake.stop.called
assert ba.shooter.stop.called
if step == 40:
return False
ba.execute() # Magicbot normally does this for us
return True
c = StepController(control, _on_step)
control.run_test(c)
assert c.step == 40
def test_shoot_boulder(control):
ba = BoulderAutomation()
ba.intake = MagicMock()
ba.shooter = MagicMock()
ba.shooter.up_to_speed = MagicMock(return_value=True)
setup_tunables(ba, "boulder_automation")
def _on_step(tm, step):
if step == 1:
ba.shoot_boulder()
assert ba.current_state == "pre_fire"
elif step == 2:
# Requires a step for the test for up_to_speed to return true
assert ba.shooter.shoot.called
elif step <= 20:
assert ba.current_state == "firing"
assert ba.is_executing
assert ba.intake.intake.called
if step > 30: # Should inject ball for around 25 steps - 0.5s
assert not ba.is_executing
assert ba.intake.stop.called
assert ba.shooter.stop.called
if step == 40:
return False
ba.execute() # Magicbot normally does this for us
return True
c = StepController(control, _on_step)
control.run_test(c)
assert c.step == 40
def test_autonomous_sm_end_timed_state(wpitime):
class _TM(AutonomousStateMachine):
i = 0
j = 0
VERBOSE_LOGGING = False
@state(first=True)
def something(self):
self.i += 1
if self.i == 3:
self.next_state("timed")
@timed_state(duration=1)
def timed(self):
self.j += 1
sm = _TM()
setup_tunables(sm, "cname")
sm.on_enable()
for _ in range(5):
wpitime.now += 0.7
sm.on_iteration(None)
assert sm.is_executing
for _ in range(5):
wpitime.now += 0.7
sm.on_iteration(None)
assert not sm.is_executing
assert sm.i == 3
assert sm.j == 2
def test_nearest_angle():
dt = Drivetrain()
setup_tunables(dt, "Drivetrain")
with patch("components.drivetrain.Drivetrain.current_angle",
new_callable=PropertyMock) as d:
d.return_value = 360
a = dt.find_nearest_angle(30)
assert a == 390
def test_angle_difference():
dt = Drivetrain()
setup_tunables(dt, "Drivetrain")
with patch("components.drivetrain.Drivetrain.current_angle",
new_callable=PropertyMock) as d:
d.return_value = 360
dt.target_angle = 390
a = dt.angle_difference
assert a == 30
def test_backdrive_manual():
ba = BoulderAutomation()
ba.intake = MagicMock()
ba.shooter = MagicMock()
setup_tunables(ba, "boulder_automation")
ba.engage("backdrive_manual")
ba.execute()
assert ba.intake.backdrive_slow.called
assert ba.shooter.backdrive_recovery.called
ba.done()
def test_backdrive_manual():
ba = BoulderAutomation()
ba.intake = MagicMock()
ba.shooter = MagicMock()
setup_tunables(ba, "boulder_automation")
ba.engage("backdrive_manual")
ba.execute()
assert ba.intake.backdrive_slow.called
assert ba.shooter.backdrive_recovery.called
ba.done()
def test_toggle_shoot_boulder():
ba = BoulderAutomation()
ba.intake = MagicMock()
ba.shooter = MagicMock()
setup_tunables(ba, "boulder_automation")
assert not ba.current_state
ba.toggle_shoot_boulder()
ba.execute()
assert ba.current_state
ba.toggle_shoot_boulder()
ba.execute()
assert not ba.current_state
def test_toggle_shoot_boulder():
ba = BoulderAutomation()
ba.intake = MagicMock()
ba.shooter = MagicMock()
setup_tunables(ba, "boulder_automation")
assert not ba.current_state
ba.toggle_shoot_boulder()
ba.execute()
assert ba.current_state
ba.toggle_shoot_boulder()
ba.execute()
assert not ba.current_state
def test_short_timed_state(wpitime):
'''
Tests two things:
- A timed state that expires before it executes
- Ensures that the default state won't execute if the machine is always
executing
'''
class _SM(StateMachine):
def __init__(self):
self.executed = []
@default_state
def d(self):
self.executed.append('d')
@state(first=True)
def a(self):
self.executed.append('a')
self.next_state('b')
@timed_state(duration=.01)
def b(self):
self.executed.append('b')
def done(self):
super().done()
self.executed.append('d')
sm = _SM()
setup_tunables(sm, 'cname')
assert sm.current_state == ''
assert not sm.is_executing
for _ in [1, 2, 3, 4]:
sm.engage()
sm.execute()
assert sm.current_state == 'b'
wpitime.now += 0.02
sm.engage()
sm.execute()
assert sm.current_state == 'b'
wpitime.now += 0.02
assert sm.executed == [
'a', 'b', 'd', 'a', 'b', 'd', 'a', 'b', 'd', 'a', 'b'
]
def test_setpoint():
e = Elevator()
setup_tunables(e, "Elevator")
e.move_to_setpoint(0)
assert e.target_height == 0, "Target height should be set to the value"
e.move_to_setpoint(30)
assert e.target_height == 30, "Target height should be set to the value"
e.move_to_setpoint(e.max_height + 50)
assert e.target_height == e.max_height, "Target height should be the max height"
e.move_to_setpoint(-5)
assert e.target_height == 0, "Target height should be the minimum value"
def test_short_timed_state(wpitime):
"""
Tests two things:
- A timed state that expires before it executes
- Ensures that the default state won't execute if the machine is always
executing
"""
class _SM(StateMachine):
def __init__(self):
self.executed = []
@default_state
def d(self):
self.executed.append("d")
@state(first=True)
def a(self):
self.executed.append("a")
self.next_state("b")
@timed_state(duration=0.01)
def b(self):
self.executed.append("b")
def done(self):
super().done()
self.executed.append("d")
sm = _SM()
setup_tunables(sm, "cname")
assert sm.current_state == ""
assert not sm.is_executing
for _ in [1, 2, 3, 4]:
sm.engage()
sm.execute()
assert sm.current_state == "b"
wpitime.now += 0.02
sm.engage()
sm.execute()
assert sm.current_state == "b"
wpitime.now += 0.02
assert sm.executed == ["a", "b", "d", "a", "b", "d", "a", "b", "d", "a", "b"]
def test_sm_inheritance():
class _TM1(StateMachine):
@state
def second_state(self):
self.done()
class _TM2(_TM1):
@state(first=True)
def first_state(self):
self.next_state("second_state")
sm = _TM2()
setup_tunables(sm, "cname")
sm.engage()
assert sm.current_state == "first_state"
sm.execute()
assert sm.current_state == "second_state"
sm.execute()
assert sm.current_state == ""
def test_next_fn():
class _TM(StateMachine):
@state(first=True)
def first_state(self):
self.next_state(self.second_state)
@state
def second_state(self):
self.done()
sm = _TM()
setup_tunables(sm, 'cname')
sm.engage()
assert sm.current_state == 'first_state'
sm.execute()
assert sm.current_state == 'second_state'
sm.engage()
sm.execute()
assert sm.current_state == ''
def test_intake_boulder(control):
ba = BoulderAutomation()
ba.intake = MagicMock()
ba.shooter = MagicMock()
ba.intake.up_to_speed = MagicMock(return_value=True)
ba.intake.ball_detected = MagicMock(return_value=True)
ba.intake.slowing = MagicMock(return_value=True)
ba.intake.pinned = MagicMock(return_value=True)
setup_tunables(ba, "boulder_automation")
def _on_step(tm, step):
if step == 1:
ba.intake_boulder()
assert ba.current_state == "pre_intake"
elif step == 2:
assert ba.intake.intake.called
assert ba.current_state == "intaking"
elif step == 3:
pass
elif step <= 29:
assert ba.shooter.backdrive.called
assert ba.current_state == "intaking_contact"
elif step == 30:
assert ba.current_state == "pinning"
elif step == 31:
assert ba.shooter.backdrive.called
assert ba.intake.backdrive_pin.called
assert ba.current_state == "pinned"
elif step == 32:
assert not ba.current_state
else:
return False
ba.execute() # Magicbot normally does this for us
return True
c = StepController(control, _on_step)
control.run_test(c)
assert c.step == 33
def test_intake_boulder(control):
ba = BoulderAutomation()
ba.intake = MagicMock()
ba.shooter = MagicMock()
ba.intake.up_to_speed = MagicMock(return_value=True)
ba.intake.ball_detected = MagicMock(return_value=True)
ba.intake.slowing = MagicMock(return_value=True)
ba.intake.pinned = MagicMock(return_value=True)
setup_tunables(ba, "boulder_automation")
def _on_step(tm, step):
if step == 1:
ba.intake_boulder()
assert ba.current_state == "pre_intake"
elif step == 2:
assert ba.intake.intake.called
assert ba.current_state == "intaking"
elif step == 3:
pass
elif step <= 29:
assert ba.shooter.backdrive.called
assert ba.current_state == "intaking_contact"
elif step == 30:
assert ba.current_state == "pinning"
elif step == 31:
assert ba.shooter.backdrive.called
assert ba.intake.backdrive_pin.called
assert ba.current_state == "pinned"
elif step == 32:
assert not ba.current_state
else:
return False
ba.execute() # Magicbot normally does this for us
return True
c = StepController(control, _on_step)
control.run_test(c)
assert c.step == 33
def test_intake():
c = Cargo()
setup_tunables(c, "Cargo")
c.intake()
assert c.motor_speed == c.motor_input_speed
def test_outtake():
c = Cargo()
setup_tunables(c, "Cargo")
c.outtake()
assert c.motor_speed == c.motor_output_speed
def test_lift():
c = Cargo()
setup_tunables(c, "Cargo")
c.lift()
assert c.is_in_position == False
def test_sm(wpitime):
class _TM(StateMachine):
def __init__(self):
self.executed = []
def some_fn(self):
self.executed.append('sf')
@state(first=True)
def first_state(self):
self.executed.append(1)
self.next_state('second_state')
@timed_state(duration=1, next_state='third_state')
def second_state(self):
self.executed.append(2)
@state
def third_state(self):
self.executed.append(3)
sm = _TM()
setup_tunables(sm, 'cname')
sm.some_fn()
# should not be able to directly call
with pytest.raises(IllegalCallError):
sm.first_state()
assert sm.current_state == ''
assert not sm.is_executing
sm.engage()
assert sm.current_state == 'first_state'
assert not sm.is_executing
sm.execute()
assert sm.current_state == 'second_state'
assert sm.is_executing
# should not change
sm.engage()
assert sm.current_state == 'second_state'
assert sm.is_executing
sm.execute()
assert sm.current_state == 'second_state'
assert sm.is_executing
wpitime.now += 1.5
sm.engage()
sm.execute()
assert sm.current_state == 'third_state'
assert sm.is_executing
sm.engage()
sm.execute()
assert sm.current_state == 'third_state'
assert sm.is_executing
# should be done
sm.done()
assert sm.current_state == ''
assert not sm.is_executing
# should be able to start directly at second state
sm.engage(initial_state='second_state')
sm.execute()
assert sm.current_state == 'second_state'
assert sm.is_executing
wpitime.now += 1.5
sm.engage()
sm.execute()
assert sm.current_state == 'third_state'
assert sm.is_executing
# test force
sm.engage()
sm.execute()
assert sm.current_state == 'third_state'
assert sm.is_executing
sm.engage(force=True)
assert sm.current_state == 'first_state'
assert sm.is_executing
sm.execute()
sm.execute()
assert not sm.is_executing
assert sm.current_state == ''
assert sm.executed == ['sf', 1, 2, 3, 3, 2, 3, 3, 1]
def test_hold():
h = Hatch()
setup_tunables(h, "Hatch")
h.hold()
assert h.is_holding == True
def test_must_finish(wpitime):
class _TM(StateMachine):
def __init__(self):
self.executed = []
@state(first=True)
def ordinary1(self):
self.next_state('ordinary2')
self.executed.append(1)
@state
def ordinary2(self):
self.next_state('must_finish')
self.executed.append(2)
@state(must_finish=True)
def must_finish(self):
self.executed.append('mf')
@state
def ordinary3(self):
self.executed.append(3)
self.next_state_now('timed_must_finish')
@timed_state(duration=1, must_finish=True)
def timed_must_finish(self):
self.executed.append('tmf')
sm = _TM()
setup_tunables(sm, 'cname')
sm.engage()
sm.execute()
sm.execute()
assert sm.current_state == ''
assert not sm.is_executing
sm.engage()
sm.execute()
sm.engage()
sm.execute()
sm.execute()
sm.execute()
assert sm.current_state == 'must_finish'
assert sm.is_executing
sm.next_state('ordinary3')
sm.engage()
sm.execute()
assert sm.current_state == 'timed_must_finish'
sm.execute()
assert sm.is_executing
assert sm.current_state == 'timed_must_finish'
for _ in range(7):
wpitime.now += 0.1
sm.execute()
assert sm.is_executing
assert sm.current_state == 'timed_must_finish'
wpitime.now += 1
sm.execute()
assert not sm.is_executing
assert sm.executed == [1, 1, 2, 'mf', 'mf', 3] + ['tmf']*9
def test_release():
h = Hatch()
setup_tunables(h, "Hatch")
h.release()
assert h.is_holding == False
def test_lift():
h = Hatch()
setup_tunables(h, "Hatch")
h.lift()
assert h.is_in_position == False
def test_autonomous_state_machine(control):
dx = 1
dy = -1
heading = math.pi/3
portcullis=True
class TestAuto(ObstacleHighGoal):
MODE_NAME = "Test Auto"
DEFAULT = True
def __init__(self):
super().__init__(dx, dy, heading, True)
#arbitary values, just so we have something to test against
a = TestAuto()
a.chassis = MagicMock()
a.shooter = MagicMock
a.shooter.shoot = MagicMock()
a.intake = MagicMock()
a.defeater = MagicMock()
a.defeater_motor = MagicMock()
a.defeater_motor.set = MagicMock()
a.bno055 = MagicMock()
a.boulder_automation = MagicMock()
a.boulder_automation.shoot_boulder = MagicMock()
a.chassis.distance_pid.onTarget = MagicMock(return_value=False)
a.chassis.heading_hold_pid.onTarget = MagicMock(return_value=False)
a.chassis.on_range_target = MagicMock(return_value=False)
a.chassis.on_vision_target = MagicMock(return_value=False)
setup_tunables(a, "autonomous")
def _on_step(tm, step):
if step == 1:
a.engage()
assert a.current_state == "deploy_defeater"
elif step == 2:
assert a.defeater_motor.set.called
assert a.chassis.distance_pid.setOutputRange.called
a.chassis.field_displace.assert_called_with(a.straight, 0.0)
assert a.current_state == "breach_defence"
elif step == 3:
assert a.current_state == "breach_defence"
a.chassis.distance_pid.onTarget = MagicMock(return_value=True)
elif step == 4:
assert a.chassis.heading_hold_pid.setSetpoint.called
assert a.defeater_motor.set.callled
assert a.current_state == "spinning"
elif step == 5:
assert a.current_state == "spinning"
a.chassis.heading_hold_pid.onTarget = MagicMock(return_value=True)
elif step == 6:
a.chassis.field_displace.assert_called_with(dx, dy)
assert a.current_state == "strafing"
a.chassis.distance_pid.onTarget = MagicMock(return_value=False)
elif step == 7:
assert a.current_state == "strafing"
a.chassis.distance_pid.onTarget = MagicMock(return_value=True)
elif step == 8:
assert a.current_state == "range_finding"
assert a.chassis.distance_pid.setOutputRange.called
assert a.chassis.distance_pid.reset.called
assert a.chassis.zero_encoders.called
assert a.chassis.range_setpoint == a.chassis.correct_range
elif step == 9:
assert a.current_state == "range_finding"
a.chassis.on_range_target = MagicMock(return_value=True)
elif step == 10:
a.chassis.on_range_target = MagicMock(return_value=False)
assert a.current_state == "visual_tracking"
assert a.chassis.track_vision
assert a.chassis.distance_pid.reset.called
a.chassis.distance_pid.setSetpoint.assert_called_with(0.0)
assert a.chassis.zero_encoders.called
assert a.chassis.distance_pid.enable.called
assert a.shooter.shoot.called
elif step == 11:
assert a.current_state == "visual_tracking"
a.chassis.on_range_target = MagicMock(return_value=True)
a.chassis.on_vision_target = MagicMock(return_value=True)
elif step == 12:
assert a.boulder_automation.shoot_boulder.called
assert not a.current_state
else:
return False
a.execute() # Magicbot normally does this for us
return True
c = StepController(control, _on_step)
control.run_test(c)
assert c.step == 13
def test_lower():
c = Cargo()
setup_tunables(c, "Cargo")
c.lower()
assert c.is_in_position == True
def test_lower():
h = Hatch()
setup_tunables(h, "Hatch")
h.lower()
assert h.is_in_position == True
def test_default_state_machine():
class _SM(StateMachine):
def __init__(self):
self.didOne = None
self.didDefault = None
self.defaultInit = None
self.didDone = None
@state(first=True)
def stateOne(self):
self.didOne = True
self.didDefault = False
self.didDone = False
@state
def doneState(self):
self.didOne = False
self.didDefault = False
self.didDone = True
self.done()
@default_state
def defaultState(self, initial_call):
self.didOne = False
self.didDefault = True
self.defaultInit = initial_call
self.didDone = False
sm = _SM()
setup_tunables(sm, 'cname')
sm.execute()
assert sm.didOne == False
assert sm.didDefault == True
assert sm.defaultInit == True
assert sm.didDone == False
sm.execute()
assert sm.didOne == False
assert sm.didDefault == True
assert sm.defaultInit == False
assert sm.didDone == False
# do a thing
sm.engage()
sm.execute()
assert sm.didOne == True
assert sm.didDefault == False
assert sm.didDone == False
# should go back (test for initial)
sm.execute()
assert sm.didOne == False
assert sm.didDefault == True
assert sm.defaultInit == True
assert sm.didDone == False
# should happen again (no initial)
sm.execute()
assert sm.didOne == False
assert sm.didDefault == True
assert sm.defaultInit == False
assert sm.didDone == False
# do another thing
sm.engage()
sm.execute()
assert sm.didOne == True
assert sm.didDefault == False
assert sm.didDone == False
# should go back (test for initial)
sm.execute()
assert sm.didOne == False
assert sm.didDefault == True
assert sm.defaultInit == True
assert sm.didDone == False
# should happen again (no initial)
sm.execute()
assert sm.didOne == False
assert sm.didDefault == True
assert sm.defaultInit == False
assert sm.didDone == False
# enagage a state that will call done, check to see
# if we come back
sm.engage("doneState")
sm.execute()
assert sm.didOne == False
assert sm.didDefault == False
assert sm.defaultInit == False
assert sm.didDone == True
# should go back (test for initial)
sm.execute()
assert sm.didOne == False
assert sm.didDefault == True
assert sm.defaultInit == True
assert sm.didDone == False
# should happen again (no initial)
sm.execute()
assert sm.didOne == False
assert sm.didDefault == True
assert sm.defaultInit == False
assert sm.didDone == False
def test_rotate_to_angle():
dt = Drivetrain()
setup_tunables(dt, "Drivetrain")
dt.rotate_to_angle(80)
assert dt.target_angle == 80
assert dt.using_gyro == True
def test_must_finish(wpitime):
class _TM(StateMachine):
def __init__(self):
self.executed = []
@state(first=True)
def ordinary1(self):
self.next_state("ordinary2")
self.executed.append(1)
@state
def ordinary2(self):
self.next_state("must_finish")
self.executed.append(2)
@state(must_finish=True)
def must_finish(self):
self.executed.append("mf")
@state
def ordinary3(self):
self.executed.append(3)
self.next_state_now("timed_must_finish")
@timed_state(duration=1, must_finish=True)
def timed_must_finish(self):
self.executed.append("tmf")
sm = _TM()
setup_tunables(sm, "cname")
sm.engage()
sm.execute()
sm.execute()
assert sm.current_state == ""
assert not sm.is_executing
sm.engage()
sm.execute()
sm.engage()
sm.execute()
sm.execute()
sm.execute()
assert sm.current_state == "must_finish"
assert sm.is_executing
sm.next_state("ordinary3")
sm.engage()
sm.execute()
assert sm.current_state == "timed_must_finish"
sm.execute()
assert sm.is_executing
assert sm.current_state == "timed_must_finish"
for _ in range(7):
wpitime.now += 0.1
sm.execute()
assert sm.is_executing
assert sm.current_state == "timed_must_finish"
wpitime.now += 1
sm.execute()
assert not sm.is_executing
assert sm.executed == [1, 1, 2, "mf", "mf", 3] + ["tmf"] * 9
