2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Diff: main.cpp
- Revision:
- 20:0ede3818e08e
- Parent:
- 19:0a3ee31dcdb4
- Child:
- 21:d6a46315d5f5
--- a/main.cpp Mon Oct 12 13:47:21 2015 +0000 +++ b/main.cpp Mon Oct 12 14:02:20 2015 +0000 @@ -88,14 +88,24 @@ } //Input error and Kp, Kd, Ki, output control signal -void pid() +double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev) { + + // Derivative + double e_der = (error − e_prev)/CONTROL_RATE; + e_der = biquad( e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2 ); + e_prev = e; + // Integral + e_int = e_int + CONTROL_RATE ∗ error; + // PID + return Kp ∗ e + Ki ∗ e_int + Kd ∗ e_der; + } -//Analyse filtered EMG, calculate reference position from EMG, compare reference position with current position,convert to angles, send error through pid(), send PWM and DIR to motors +//Analyze filtered EMG, calculate reference position from EMG, compare reference position with current position,convert to angles, send error through pid(), send PWM and DIR to motors void control() { - //analyse emg (= velocity, averages?) + //analyze emg (= velocity, averages?) //calculate reference position based on the average emg (integrate)