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_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_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(): 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