Added working robot
Working stabilization loop
Merge branch 'working'