Changed up state machine
Merge branch 'golnaz_old' of https://github.com/mrsl/rone into golnaz_old

Conflicts:
	software/projects/MultiRobotManipulation/src/COMandTreeTesting.c