State Machine works
Merged to get grasp point
Tested autonomous code for Left arm, planning to switch tools