Working on a impedance controller, this has an OK(?) PD controller on motor
Kind of works... I think. Oscillates a lot, but does what I think it should do??