Recovered from the mess from milestone2 and fixed bugs in logic, control_motion and mapping. Works good.
Added mapping to theia package
Started work to detect perimeter closure.