Can control arm using x,y,z coordinates
1. invK produces 8 different possibilities - not sure which one to choose
2. Code divided into arm control and arm comms