2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
main.cpp@20:0ede3818e08e, 2015-10-12 (annotated)
- Committer:
- Vigilance88
- Date:
- Mon Oct 12 14:02:20 2015 +0000
- Revision:
- 20:0ede3818e08e
- Parent:
- 19:0a3ee31dcdb4
- Child:
- 21:d6a46315d5f5
reusable pid
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsluiter | 0:32bb76391d89 | 1 | #include "mbed.h" |
vsluiter | 11:ce72ec658a95 | 2 | #include "HIDScope.h" |
Vigilance88 | 18:44905b008f44 | 3 | #include "MODSERIAL.h" |
Vigilance88 | 18:44905b008f44 | 4 | #include "biquadFilter.h" |
Vigilance88 | 18:44905b008f44 | 5 | #include "QEI.h" |
vsluiter | 0:32bb76391d89 | 6 | |
Vigilance88 | 18:44905b008f44 | 7 | //Define important constants in memory |
Vigilance88 | 18:44905b008f44 | 8 | #define SAMPLE_RATE 0.002 //500 Hz EMG sample rate |
Vigilance88 | 18:44905b008f44 | 9 | #define CONTROL_RATE 0.01 //100 Hz Control rate |
Vigilance88 | 18:44905b008f44 | 10 | #define ENCODER1_CPR 4200 //encoders have 4200 counts per revolution of the output shaft (X2) |
Vigilance88 | 18:44905b008f44 | 11 | #define ENCODER2_CPR 4200 //64 X4, 32 X4 counts per revolution of motor shaft, gearbox 1:131.25 |
Vigilance88 | 18:44905b008f44 | 12 | |
Vigilance88 | 18:44905b008f44 | 13 | //Objects |
Vigilance88 | 18:44905b008f44 | 14 | MODSERIAL pc(USBTX,USBRX); //serial communication |
Vigilance88 | 18:44905b008f44 | 15 | DigitalIn button(PTA1); //buttons |
Vigilance88 | 18:44905b008f44 | 16 | DigitalIn button1(PTB9); // |
Vigilance88 | 18:44905b008f44 | 17 | |
Vigilance88 | 18:44905b008f44 | 18 | AnalogIn emg1(A0); //Analog input - Biceps EMG |
Vigilance88 | 18:44905b008f44 | 19 | AnalogIn emg2(A1); //Analog input - Triceps EMG |
Vigilance88 | 18:44905b008f44 | 20 | AnalogIn emg3(A2); //Analog input - Flexor EMG |
Vigilance88 | 18:44905b008f44 | 21 | AnalogIn emg4(A3); //Analog input - Extensor EMG |
Vigilance88 | 18:44905b008f44 | 22 | |
Vigilance88 | 18:44905b008f44 | 23 | Ticker sample_timer; //Ticker for EMG sampling |
Vigilance88 | 18:44905b008f44 | 24 | Ticker control_timer; //Ticker for control loop |
Vigilance88 | 18:44905b008f44 | 25 | HIDScope scope(4); //Scope 4 channels |
Vigilance88 | 18:44905b008f44 | 26 | |
Vigilance88 | 18:44905b008f44 | 27 | // AnalogIn potmeter(A4); //potmeters |
Vigilance88 | 18:44905b008f44 | 28 | // AnalogIn potmeter2(A5); // |
Vigilance88 | 18:44905b008f44 | 29 | |
Vigilance88 | 18:44905b008f44 | 30 | QEI Encoder1(D13,D12,NC,32); //channel A and B from encoder, counts = Encoder.getPulses(); |
Vigilance88 | 18:44905b008f44 | 31 | QEI Encoder2(D10,D9,NC,32); //channel A and B from encoder, |
Vigilance88 | 18:44905b008f44 | 32 | PwmOut pwm_motor1(D6); //D4 and D5 = motor 2, D7 and D6 = motor 1 |
Vigilance88 | 18:44905b008f44 | 33 | PwmOut pwm_motor2(D5); //D4 and D5 = motor 2, D7 and D6 = motor 1 |
Vigilance88 | 18:44905b008f44 | 34 | DigitalOut dir_motor1(D7); //Direction motor 1 |
Vigilance88 | 18:44905b008f44 | 35 | DigitalOut dir_motor2(D4); //Direction motor 2 |
Vigilance88 | 18:44905b008f44 | 36 | |
Vigilance88 | 18:44905b008f44 | 37 | //Filters |
Vigilance88 | 18:44905b008f44 | 38 | //biquadFilter filter(a1, a2, b0, b1, b2); coefficients |
Vigilance88 | 18:44905b008f44 | 39 | biquadFilter filter1( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); //eg. notch / lowpass / highpass |
Vigilance88 | 18:44905b008f44 | 40 | biquadFilter filter2( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); //eg. notch / lowpass / highpass |
Vigilance88 | 18:44905b008f44 | 41 | biquadFilter filter3( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); //eg. notch / lowpass / highpass |
Vigilance88 | 18:44905b008f44 | 42 | biquadFilter filter4( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); //eg. notch / lowpass / highpass |
Vigilance88 | 18:44905b008f44 | 43 | |
Vigilance88 | 18:44905b008f44 | 44 | void keep_in_range(float * in, float min, float max); |
Vigilance88 | 18:44905b008f44 | 45 | void sample(void); |
Vigilance88 | 18:44905b008f44 | 46 | |
Vigilance88 | 18:44905b008f44 | 47 | //Start sampling |
Vigilance88 | 18:44905b008f44 | 48 | void start_sampling(void) |
Vigilance88 | 18:44905b008f44 | 49 | { |
Vigilance88 | 18:44905b008f44 | 50 | sample_timer.attach(&sample,SAMPLE_RATE); |
Vigilance88 | 18:44905b008f44 | 51 | } |
Vigilance88 | 18:44905b008f44 | 52 | |
Vigilance88 | 18:44905b008f44 | 53 | //stop sampling |
Vigilance88 | 18:44905b008f44 | 54 | void stop_sampling(void) |
Vigilance88 | 18:44905b008f44 | 55 | { |
Vigilance88 | 18:44905b008f44 | 56 | sample_timer.detach(); |
Vigilance88 | 18:44905b008f44 | 57 | } |
vsluiter | 2:e314bb3b2d99 | 58 | |
Vigilance88 | 18:44905b008f44 | 59 | //Sample function |
Vigilance88 | 18:44905b008f44 | 60 | void sample(void) |
Vigilance88 | 18:44905b008f44 | 61 | { |
Vigilance88 | 18:44905b008f44 | 62 | double emg_value1 = emg1.read(); |
Vigilance88 | 18:44905b008f44 | 63 | double emg_value2 = emg2.read(); |
Vigilance88 | 18:44905b008f44 | 64 | double emg_value3 = emg3.read(); |
Vigilance88 | 18:44905b008f44 | 65 | double emg_value4 = emg4.read(); |
Vigilance88 | 18:44905b008f44 | 66 | scope.set(0,emg_value1); |
Vigilance88 | 18:44905b008f44 | 67 | scope.set(1,emg_value2); |
Vigilance88 | 18:44905b008f44 | 68 | scope.set(2,emg_value3); |
Vigilance88 | 18:44905b008f44 | 69 | scope.set(3,emg_value4); |
Vigilance88 | 18:44905b008f44 | 70 | scope.send(); |
Vigilance88 | 18:44905b008f44 | 71 | } |
Vigilance88 | 18:44905b008f44 | 72 | |
Vigilance88 | 18:44905b008f44 | 73 | //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position. |
Vigilance88 | 19:0a3ee31dcdb4 | 74 | void calibrate_arm(void) |
Vigilance88 | 19:0a3ee31dcdb4 | 75 | { |
Vigilance88 | 19:0a3ee31dcdb4 | 76 | } |
Vigilance88 | 19:0a3ee31dcdb4 | 77 | |
Vigilance88 | 19:0a3ee31dcdb4 | 78 | //EMG MVC measurement |
Vigilance88 | 19:0a3ee31dcdb4 | 79 | void calibrate_emg(void) |
Vigilance88 | 18:44905b008f44 | 80 | { |
Vigilance88 | 18:44905b008f44 | 81 | } |
Vigilance88 | 18:44905b008f44 | 82 | |
Vigilance88 | 18:44905b008f44 | 83 | //use biquadFilter library, output filtered EMG (combine with sample?) |
Vigilance88 | 18:44905b008f44 | 84 | double filter(double u) |
Vigilance88 | 18:44905b008f44 | 85 | { |
Vigilance88 | 18:44905b008f44 | 86 | double test; |
Vigilance88 | 18:44905b008f44 | 87 | return test; |
Vigilance88 | 18:44905b008f44 | 88 | } |
Vigilance88 | 18:44905b008f44 | 89 | |
Vigilance88 | 18:44905b008f44 | 90 | //Input error and Kp, Kd, Ki, output control signal |
Vigilance88 | 20:0ede3818e08e | 91 | double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev) |
vsluiter | 2:e314bb3b2d99 | 92 | { |
Vigilance88 | 20:0ede3818e08e | 93 | |
Vigilance88 | 20:0ede3818e08e | 94 | // Derivative |
Vigilance88 | 20:0ede3818e08e | 95 | double e_der = (error − e_prev)/CONTROL_RATE; |
Vigilance88 | 20:0ede3818e08e | 96 | e_der = biquad( e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2 ); |
Vigilance88 | 20:0ede3818e08e | 97 | e_prev = e; |
Vigilance88 | 20:0ede3818e08e | 98 | // Integral |
Vigilance88 | 20:0ede3818e08e | 99 | e_int = e_int + CONTROL_RATE ∗ error; |
Vigilance88 | 20:0ede3818e08e | 100 | // PID |
Vigilance88 | 20:0ede3818e08e | 101 | return Kp ∗ e + Ki ∗ e_int + Kd ∗ e_der; |
Vigilance88 | 20:0ede3818e08e | 102 | |
Vigilance88 | 18:44905b008f44 | 103 | } |
Vigilance88 | 18:44905b008f44 | 104 | |
Vigilance88 | 20:0ede3818e08e | 105 | //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 |
Vigilance88 | 18:44905b008f44 | 106 | void control() |
Vigilance88 | 18:44905b008f44 | 107 | { |
Vigilance88 | 20:0ede3818e08e | 108 | //analyze emg (= velocity, averages?) |
Vigilance88 | 18:44905b008f44 | 109 | |
Vigilance88 | 18:44905b008f44 | 110 | //calculate reference position based on the average emg (integrate) |
Vigilance88 | 18:44905b008f44 | 111 | |
Vigilance88 | 18:44905b008f44 | 112 | //calculate error (refpos-currentpos) currentpos = forward kinematics |
Vigilance88 | 18:44905b008f44 | 113 | |
Vigilance88 | 18:44905b008f44 | 114 | //inverse kinematics (pos error to angle error) |
Vigilance88 | 18:44905b008f44 | 115 | |
Vigilance88 | 18:44905b008f44 | 116 | //PID controller |
Vigilance88 | 18:44905b008f44 | 117 | pid(); |
Vigilance88 | 18:44905b008f44 | 118 | //send PWM and DIR to motors |
Vigilance88 | 18:44905b008f44 | 119 | } |
Vigilance88 | 18:44905b008f44 | 120 | |
Vigilance88 | 18:44905b008f44 | 121 | //Start control |
Vigilance88 | 18:44905b008f44 | 122 | void start_control(void) |
Vigilance88 | 18:44905b008f44 | 123 | { |
Vigilance88 | 18:44905b008f44 | 124 | control_timer.attach(&control,SAMPLE_RATE); |
Vigilance88 | 18:44905b008f44 | 125 | } |
Vigilance88 | 18:44905b008f44 | 126 | |
Vigilance88 | 18:44905b008f44 | 127 | //stop control |
Vigilance88 | 18:44905b008f44 | 128 | void stop_control(void) |
Vigilance88 | 18:44905b008f44 | 129 | { |
Vigilance88 | 18:44905b008f44 | 130 | control_timer.detach(); |
vsluiter | 2:e314bb3b2d99 | 131 | } |
vsluiter | 0:32bb76391d89 | 132 | |
vsluiter | 0:32bb76391d89 | 133 | int main() |
vsluiter | 0:32bb76391d89 | 134 | { |
Vigilance88 | 18:44905b008f44 | 135 | // make a menu, user has to initiated next step |
Vigilance88 | 18:44905b008f44 | 136 | |
Vigilance88 | 19:0a3ee31dcdb4 | 137 | calibrate_arm(); //Start Calibration |
Vigilance88 | 19:0a3ee31dcdb4 | 138 | calibrate_emg(); |
Vigilance88 | 18:44905b008f44 | 139 | start_sampling(); //500 Hz EMG |
Vigilance88 | 18:44905b008f44 | 140 | start_control(); //100 Hz control |
Vigilance88 | 18:44905b008f44 | 141 | |
Vigilance88 | 18:44905b008f44 | 142 | //maybe some stop commands when a button is pressed: detach from timers. |
Vigilance88 | 18:44905b008f44 | 143 | //stop_control(); |
Vigilance88 | 18:44905b008f44 | 144 | //stop_sampling(); |
Vigilance88 | 18:44905b008f44 | 145 | |
Vigilance88 | 18:44905b008f44 | 146 | while(1) { |
Vigilance88 | 18:44905b008f44 | 147 | |
Vigilance88 | 18:44905b008f44 | 148 | |
Vigilance88 | 18:44905b008f44 | 149 | }//end of while loop |
Vigilance88 | 18:44905b008f44 | 150 | }//end of main |
tomlankhorst | 15:0da764eea774 | 151 | |
Vigilance88 | 18:44905b008f44 | 152 | void keep_in_range(float * in, float min, float max) |
Vigilance88 | 18:44905b008f44 | 153 | { |
Vigilance88 | 18:44905b008f44 | 154 | *in > min ? *in < max? : *in = max: *in = min; |
vsluiter | 0:32bb76391d89 | 155 | } |