Signal Lost Start Spinning in the ‘opposite direction of the last saved motor direction, Set goal to 1 Ultrasound or Bump not a threshold ‘Stop Moving, Set goal to 2 Initialization Start Spinning Goal 1: Find 1 kHz Signal ¥ Event: Signal Found ‘Action: Stop Spinning, Save motor direction Set goal to 2 ¥ Uttrasound or Bump not at threshold, \Goal 2: Move towards| 1 kHz True 1 kHz signal oe ‘Spin motors forward ¥ Untrasound or Bump sensor pings proper conditions ‘Stop Moving, Set goal to 3 ¥ Goal 3: Dump Chips Redundancy: Ultrasound or Bump sensors ping proper conditions Activate dump servos, walt for 1 second, then recall servos ‘ Set goal to 4 Goal 4: Find § kHz -——_—_> 5 kttz Signal not seen Signal ~~" — Spin nlast saved motor rection Save motor direction 5 kt signal lost 5 kHz signal found Tages —=<£€»_ Spin opposite to ast Slop spinning, sot goalto5 saved motor creation ¥ (Goal 5: Reload | > trasound or Bump not at threshold, ove towards kHz Skitz True signal) }<—_——_—_ Utrasound or Bump sensor pings proper conditions Stop moving, wait for a few seconds (Reload), and set goal to 1 Encoders not changing for certain amount of clock cycles RESET POSITION Move backwards for 1 sec Move forwards for .5 sec Resume Goal ‘Spin motors forward 2 mins have passed ‘Stop all motors and serves