More updated robo code

Now the robot arm should have excellent mobility.