Code adjusted for 6 servos
FIX: Shoulder rotation calculation
FIX: Clamp changes from one state to next (max difference 0.25?)
FIX: Change Arduiono coms to send a series of bytes, NO STRINGS