Robotics Fundamentals 1, Chapter 8: Challenge 1 solution (reactive behaviours, arbitration and state machines)

Formulas: Braitenberg wheel speed = b + g x sensor ;  v = ( vL + vR ) / 2 ;  w = ( vR - vL ) / L   (counter-clockwise positive)
          priority arbitration: ask the behaviours from the highest priority down; the first one with a command wins
          state machine: a table of (state, event) -> next state;  debounce = N consecutive samples;  independent aspects multiply in one machine and add in separate machines

(a) sL = 0.2, sR = 0.7 (the light is on the RIGHT), b = 0.1, g = 0.5, L = 0.12.
    UNCROSSED (left sensor -> left wheel, right sensor -> right wheel):
        vL = 0.1 + 0.5 x 0.2 = 0.20 m/s        vR = 0.1 + 0.5 x 0.7 = 0.45 m/s
        v = ( 0.20 + 0.45 ) / 2 = 0.325 m/s     w = ( 0.45 - 0.20 ) / 0.12 = +2.083 rad/s   counter-clockwise: turns LEFT, away from the light on the right.
    CROSSED (left sensor -> right wheel, right sensor -> left wheel):
        vL = 0.1 + 0.5 x 0.7 = 0.45 m/s        vR = 0.1 + 0.5 x 0.2 = 0.20 m/s
        v = 0.325 m/s                           w = ( 0.20 - 0.45 ) / 0.12 = -2.083 rad/s    clockwise: turns RIGHT, toward the light.
    The forward speed is the same; only the sign of the turn differs, and that sign is the whole difference between avoiding and approaching.

(b) Behaviours in priority order: STOP (bump), AVOID (front < 0.30: v = 0, w = +1.0 if left > right else -1.0), LINE (v = 0.18, w = 0.08 x ( -x )), WANDER (0.15, 0).
    (i)   no bump, front 0.25, left 0.3, right 0.8, line at +4 mm:
          STOP has nothing (no bump).  AVOID: 0.25 < 0.30, so it is active, and it outranks LINE.  Left (0.3) is NOT greater than right (0.8), so w = -1.0.
          Winner AVOID, command (0.00, -1.00): stop and turn clockwise (right, toward the clearer side).
    (ii)  no bump, front 0.9, line at -10 mm:
          STOP no.  AVOID no (0.9 > 0.30).  LINE: e = -( -10 ) = +10 mm,  w = 0.08 x 10 = +0.80 rad/s.
          Winner LINE, command (0.18, +0.80): the line is on the left, so turn left.
    (iii) no bump, front 0.9, no line:
          STOP no, AVOID no, LINE returns nothing (no line seen).  WANDER always answers.
          Winner WANDER, command (0.15, 0.00).

(c) States: WAIT, TO_PICKUP, LOADING, TO_DROP, UNLOADING, HOLD_PICKUP, HOLD_DROP.   Events: order, arrived, loaded, unloaded, blocked, clear.

        State          Event       Next state
        WAIT           order       TO_PICKUP
        TO_PICKUP      arrived     LOADING
        TO_PICKUP      blocked     HOLD_PICKUP
        HOLD_PICKUP    clear       TO_PICKUP
        LOADING        loaded      TO_DROP
        TO_DROP        arrived     UNLOADING
        TO_DROP        blocked     HOLD_DROP
        HOLD_DROP      clear       TO_DROP
        UNLOADING      unloaded    WAIT
        (every other pair is ignored)

    Trace of order, arrived, loaded, blocked, clear, arrived, unloaded:
        WAIT --order--> TO_PICKUP --arrived--> LOADING --loaded--> TO_DROP --blocked--> HOLD_DROP --clear--> TO_DROP --arrived--> UNLOADING --unloaded--> WAIT
    States visited in order: WAIT, TO_PICKUP, LOADING, TO_DROP, HOLD_DROP, TO_DROP, UNLOADING, WAIT.
    Why two HOLD states: a state machine remembers only its current state's name.  After "clear" the robot has to know which trip it was on, and with a single HOLD state that information
    is gone, so the machine could not tell whether to resume TO_PICKUP or TO_DROP.  Either the memory is put in the states (two HOLD states, as here, the cost of which grows with the number of trips),
    or it is kept in a separate variable (the "previous state"), or the "blocked" condition is handled as a behaviour INSIDE each trip state instead of a state of its own.

(d) Readings every 0.2 s: 24, 19, 22, 19, 18, 19, 19, 18, 19, 19 (samples 0 to 9).  battery_low needs 4 readings in a row below 20.
        sample     0    1    2    3    4    5    6    7    8    9
        reading   24   19   22   19   18   19   19   18   19   19
        count      0    1    0    1    2    3    4    5    6    7
    The count (consecutive readings below 20) first reaches 4 at sample 6 (readings 19, 18, 19, 19 at samples 3 to 6), so battery_low fires at t = 6 x 0.2 = 1.2 s.
    The naive rule fires at the first reading below 20, sample 1, at t = 0.2 s: 1.0 s earlier, and it is a false alarm as far as the later readings go (sample 2 reads 22).

(e) One machine, every combination:  5 x 3 x 2 x 2 = 60 states.   Four separate machines, one for each independent aspect:  5 + 3 + 2 + 2 = 12 states.
    Adding one more two-valued aspect doubles the single machine to 120 states and adds only 2 to the separate set.