Found a major BUG!
Fixed multi-concurrent thread access on sensors or motors. This should increase stability and improve performance!

Added new modes: Separated color detection from init state to search state change.
Added new flee mode (not fully implemented)
Added reset to Init state functionallity