after adding the capacitor 1uF between Vmot_out and GND, the original control equation worked better. also fixed impedance control math errors

moving away from admittance and going to impedance control. added pid, discrete control design and open loop ramp rate. Eliminated a lot of dev fluff