Got arm working in open and closed loop modes

Need to clean up, but the code works decently as a test of P loops!