Started working on a state manager and a centralized place where state logic should be
Fixed output. Made turning and stopping go to timed out states correctly. Fixed buffer size to 1 instead of 0 which solved old wall distance messages coming through at a later time.
Removed turning field, and now using state manager to tell if following wall, stopping or turning
Changed default stop value of front sensors. Now also following wall again after first turn
Cleaned code a bit