Fix getCubes, more debugging

Robot now does:
1) gets to cubes
2) turns, lifts arm
3) finds cube
<picking up needs work>