Removed 3d Plotting testing code
Added R hack
Added folder for arduino
Added wait for servos to finish and loop over steps