![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
encoder
Dependencies: MODSERIAL mbed HIDScope biquadFilter
Fork of Project_script by
main.cpp@25:c719346df3cd, 2018-10-30 (annotated)
- Committer:
- MarijkeZondag
- Date:
- Tue Oct 30 21:27:35 2018 +0000
- Revision:
- 25:c719346df3cd
- Parent:
- 24:b9dd6cf5c366
- Child:
- 26:8fc7e88603f8
WERKT NOG NIET PID, gekke weerstand, motor draait wel maar we weten niet precies wat hij doet.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsluiter | 0:c8f15874531b | 1 | #include "mbed.h" |
vsluiter | 0:c8f15874531b | 2 | #include "MODSERIAL.h" |
MarijkeZondag | 24:b9dd6cf5c366 | 3 | #include "BiQuad.h" |
MarijkeZondag | 25:c719346df3cd | 4 | #include "HIDScope.h" |
MarijkeZondag | 25:c719346df3cd | 5 | #include <math.h> |
vsluiter | 0:c8f15874531b | 6 | |
MarijkeZondag | 25:c719346df3cd | 7 | //ATTENTION: set mBed to version 151 |
MarijkeZondag | 25:c719346df3cd | 8 | // set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder) |
MarijkeZondag | 25:c719346df3cd | 9 | // set MODSERIAL to version 44 |
MarijkeZondag | 25:c719346df3cd | 10 | // set HIDScope to version 7 |
MarijkeZondag | 25:c719346df3cd | 11 | // set biquadFilter to version 7 |
MarijkeZondag | 23:c027e5c57cc8 | 12 | |
MarijkeZondag | 9:c722418997b5 | 13 | InterruptIn encoderA (D9); |
MarijkeZondag | 9:c722418997b5 | 14 | InterruptIn encoderB (D8); |
MarijkeZondag | 6:f4bbb73f3989 | 15 | |
MarijkeZondag | 23:c027e5c57cc8 | 16 | DigitalOut directionpin1 (D7); |
MarijkeZondag | 23:c027e5c57cc8 | 17 | DigitalOut directionpin2 (D4); |
MarijkeZondag | 25:c719346df3cd | 18 | |
MarijkeZondag | 25:c719346df3cd | 19 | DigitalIn button2 (D10); |
MarijkeZondag | 25:c719346df3cd | 20 | |
MarijkeZondag | 23:c027e5c57cc8 | 21 | PwmOut pwmpin1 (D6); |
MarijkeZondag | 23:c027e5c57cc8 | 22 | PwmOut pwmpin2 (D5); |
MarijkeZondag | 9:c722418997b5 | 23 | |
MarijkeZondag | 6:f4bbb73f3989 | 24 | |
vsluiter | 0:c8f15874531b | 25 | MODSERIAL pc(USBTX, USBRX); |
MarijkeZondag | 25:c719346df3cd | 26 | //HIDScope scope( 3 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered |
vsluiter | 0:c8f15874531b | 27 | |
MarijkeZondag | 9:c722418997b5 | 28 | |
MarijkeZondag | 9:c722418997b5 | 29 | //Global variables |
MarijkeZondag | 25:c719346df3cd | 30 | int encoder = 1; //Start value encoder |
MarijkeZondag | 24:b9dd6cf5c366 | 31 | |
MarijkeZondag | 25:c719346df3cd | 32 | double T = 0.01f; //Ticker period |
MarijkeZondag | 25:c719346df3cd | 33 | |
MarijkeZondag | 25:c719346df3cd | 34 | double Kp = 17.5; //Motor 1 |
MarijkeZondag | 24:b9dd6cf5c366 | 35 | double Ki = 1.02; |
MarijkeZondag | 24:b9dd6cf5c366 | 36 | double Kd = 23.2; |
MarijkeZondag | 25:c719346df3cd | 37 | double PI = 3.14159; |
MarijkeZondag | 24:b9dd6cf5c366 | 38 | |
MarijkeZondag | 25:c719346df3cd | 39 | //double err = 0; |
MarijkeZondag | 25:c719346df3cd | 40 | |
MarijkeZondag | 25:c719346df3cd | 41 | volatile double reference_rotation = PI; //pi , in radians |
MarijkeZondag | 25:c719346df3cd | 42 | double encoder_radians; |
MarijkeZondag | 25:c719346df3cd | 43 | |
MarijkeZondag | 25:c719346df3cd | 44 | //double u_total = 0; |
MarijkeZondag | 25:c719346df3cd | 45 | |
MarijkeZondag | 25:c719346df3cd | 46 | double pi=3.14159; |
MarijkeZondag | 25:c719346df3cd | 47 | double i=0; |
MarijkeZondag | 24:b9dd6cf5c366 | 48 | |
MarijkeZondag | 24:b9dd6cf5c366 | 49 | //Tickers |
MarijkeZondag | 25:c719346df3cd | 50 | Ticker encoder_tick; |
MarijkeZondag | 25:c719346df3cd | 51 | Ticker engine_tick; |
MarijkeZondag | 8:895d941a5910 | 52 | |
MarijkeZondag | 9:c722418997b5 | 53 | |
MarijkeZondag | 9:c722418997b5 | 54 | //Functions |
MarijkeZondag | 25:c719346df3cd | 55 | |
MarijkeZondag | 25:c719346df3cd | 56 | /* |
MarijkeZondag | 25:c719346df3cd | 57 | void HIDScope_sample() |
MarijkeZondag | 25:c719346df3cd | 58 | { |
MarijkeZondag | 25:c719346df3cd | 59 | scope.set(0,sinout); |
MarijkeZondag | 25:c719346df3cd | 60 | scope.set(1,encoder); //alles in array? |
MarijkeZondag | 25:c719346df3cd | 61 | scope.set(2,err); |
MarijkeZondag | 25:c719346df3cd | 62 | |
MarijkeZondag | 25:c719346df3cd | 63 | scope.send(); |
MarijkeZondag | 25:c719346df3cd | 64 | } |
MarijkeZondag | 25:c719346df3cd | 65 | */ |
MarijkeZondag | 25:c719346df3cd | 66 | |
MarijkeZondag | 25:c719346df3cd | 67 | |
MarijkeZondag | 8:895d941a5910 | 68 | void encoderA_rise() |
MarijkeZondag | 8:895d941a5910 | 69 | { |
MarijkeZondag | 8:895d941a5910 | 70 | if(encoderB==false) |
MarijkeZondag | 8:895d941a5910 | 71 | { |
MarijkeZondag | 8:895d941a5910 | 72 | encoder++; |
MarijkeZondag | 8:895d941a5910 | 73 | } |
MarijkeZondag | 8:895d941a5910 | 74 | else |
MarijkeZondag | 8:895d941a5910 | 75 | { |
MarijkeZondag | 8:895d941a5910 | 76 | encoder--; |
MarijkeZondag | 8:895d941a5910 | 77 | } |
MarijkeZondag | 8:895d941a5910 | 78 | } |
MarijkeZondag | 8:895d941a5910 | 79 | |
MarijkeZondag | 8:895d941a5910 | 80 | void encoderA_fall() |
MarijkeZondag | 8:895d941a5910 | 81 | { |
MarijkeZondag | 8:895d941a5910 | 82 | if(encoderB==true) |
MarijkeZondag | 8:895d941a5910 | 83 | { |
MarijkeZondag | 8:895d941a5910 | 84 | encoder++; |
MarijkeZondag | 8:895d941a5910 | 85 | } |
MarijkeZondag | 8:895d941a5910 | 86 | else |
MarijkeZondag | 8:895d941a5910 | 87 | { |
MarijkeZondag | 8:895d941a5910 | 88 | encoder--; |
MarijkeZondag | 8:895d941a5910 | 89 | } |
MarijkeZondag | 8:895d941a5910 | 90 | } |
MarijkeZondag | 8:895d941a5910 | 91 | |
MarijkeZondag | 8:895d941a5910 | 92 | void encoderB_rise() |
MarijkeZondag | 8:895d941a5910 | 93 | { |
MarijkeZondag | 8:895d941a5910 | 94 | if(encoderA==true) |
MarijkeZondag | 8:895d941a5910 | 95 | { |
MarijkeZondag | 8:895d941a5910 | 96 | encoder++; |
MarijkeZondag | 8:895d941a5910 | 97 | } |
MarijkeZondag | 8:895d941a5910 | 98 | else |
MarijkeZondag | 8:895d941a5910 | 99 | { |
MarijkeZondag | 8:895d941a5910 | 100 | encoder--; |
MarijkeZondag | 8:895d941a5910 | 101 | } |
MarijkeZondag | 8:895d941a5910 | 102 | } |
MarijkeZondag | 8:895d941a5910 | 103 | |
MarijkeZondag | 8:895d941a5910 | 104 | void encoderB_fall() |
MarijkeZondag | 8:895d941a5910 | 105 | { |
MarijkeZondag | 8:895d941a5910 | 106 | if(encoderA==false) |
MarijkeZondag | 8:895d941a5910 | 107 | { |
MarijkeZondag | 8:895d941a5910 | 108 | encoder++; |
MarijkeZondag | 8:895d941a5910 | 109 | } |
MarijkeZondag | 8:895d941a5910 | 110 | else |
MarijkeZondag | 8:895d941a5910 | 111 | { |
MarijkeZondag | 8:895d941a5910 | 112 | encoder--; |
MarijkeZondag | 8:895d941a5910 | 113 | } |
MarijkeZondag | 8:895d941a5910 | 114 | } |
MarijkeZondag | 8:895d941a5910 | 115 | |
MarijkeZondag | 24:b9dd6cf5c366 | 116 | void encoder_count() |
MarijkeZondag | 24:b9dd6cf5c366 | 117 | { |
MarijkeZondag | 24:b9dd6cf5c366 | 118 | encoderA.rise(&encoderA_rise); |
MarijkeZondag | 24:b9dd6cf5c366 | 119 | encoderA.fall(&encoderA_fall); |
MarijkeZondag | 24:b9dd6cf5c366 | 120 | encoderB.rise(&encoderB_rise); |
MarijkeZondag | 24:b9dd6cf5c366 | 121 | encoderB.fall(&encoderB_fall); |
MarijkeZondag | 24:b9dd6cf5c366 | 122 | } |
MarijkeZondag | 24:b9dd6cf5c366 | 123 | |
MarijkeZondag | 25:c719346df3cd | 124 | double PID_controller(double err) |
MarijkeZondag | 24:b9dd6cf5c366 | 125 | { |
MarijkeZondag | 24:b9dd6cf5c366 | 126 | static double err_integral = 0; |
MarijkeZondag | 24:b9dd6cf5c366 | 127 | static double err_prev = err; // initialization with this value only done once! |
MarijkeZondag | 24:b9dd6cf5c366 | 128 | |
MarijkeZondag | 24:b9dd6cf5c366 | 129 | static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); |
MarijkeZondag | 24:b9dd6cf5c366 | 130 | |
MarijkeZondag | 24:b9dd6cf5c366 | 131 | // Proportional part: |
MarijkeZondag | 24:b9dd6cf5c366 | 132 | double u_k = Kp * err; |
MarijkeZondag | 24:b9dd6cf5c366 | 133 | |
MarijkeZondag | 24:b9dd6cf5c366 | 134 | // Integral part |
MarijkeZondag | 24:b9dd6cf5c366 | 135 | err_integral = err_integral + err * T; |
MarijkeZondag | 24:b9dd6cf5c366 | 136 | double u_i = Ki * err_integral; |
MarijkeZondag | 24:b9dd6cf5c366 | 137 | |
MarijkeZondag | 24:b9dd6cf5c366 | 138 | // Derivative part |
MarijkeZondag | 24:b9dd6cf5c366 | 139 | double err_derivative = (err - err_prev)/T; |
MarijkeZondag | 24:b9dd6cf5c366 | 140 | double filtered_err_derivative = LowPassFilter.step(err_derivative); |
MarijkeZondag | 24:b9dd6cf5c366 | 141 | double u_d = Kd * filtered_err_derivative; |
MarijkeZondag | 24:b9dd6cf5c366 | 142 | err_prev = err; |
MarijkeZondag | 24:b9dd6cf5c366 | 143 | |
MarijkeZondag | 24:b9dd6cf5c366 | 144 | // Sum all parts and return it |
MarijkeZondag | 25:c719346df3cd | 145 | return u_k + u_i + u_d; |
MarijkeZondag | 24:b9dd6cf5c366 | 146 | } |
MarijkeZondag | 24:b9dd6cf5c366 | 147 | |
MarijkeZondag | 25:c719346df3cd | 148 | void start_your_engines(double u) |
MarijkeZondag | 24:b9dd6cf5c366 | 149 | { |
MarijkeZondag | 25:c719346df3cd | 150 | if(encoder<5000 && encoder>-5000) //limieten van de translatie 11760 encoder counts |
MarijkeZondag | 25:c719346df3cd | 151 | { |
MarijkeZondag | 25:c719346df3cd | 152 | pwmpin1 = fabs(u); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! |
MarijkeZondag | 25:c719346df3cd | 153 | directionpin1.write(u > 0.0f); |
MarijkeZondag | 25:c719346df3cd | 154 | } |
MarijkeZondag | 25:c719346df3cd | 155 | else |
MarijkeZondag | 25:c719346df3cd | 156 | { |
MarijkeZondag | 25:c719346df3cd | 157 | pwmpin1 = 0; |
MarijkeZondag | 25:c719346df3cd | 158 | } |
MarijkeZondag | 25:c719346df3cd | 159 | } |
MarijkeZondag | 25:c719346df3cd | 160 | |
MarijkeZondag | 25:c719346df3cd | 161 | void engine_control() |
MarijkeZondag | 25:c719346df3cd | 162 | { |
MarijkeZondag | 25:c719346df3cd | 163 | encoder_radians = encoder*(2*PI)/8400; |
MarijkeZondag | 25:c719346df3cd | 164 | double err = reference_rotation - encoder_radians; |
MarijkeZondag | 25:c719346df3cd | 165 | double u = PID_controller(err); //PID controller function call |
MarijkeZondag | 25:c719346df3cd | 166 | start_your_engines(u); //Call start_your_engines function |
MarijkeZondag | 24:b9dd6cf5c366 | 167 | } |
MarijkeZondag | 8:895d941a5910 | 168 | |
MarijkeZondag | 9:c722418997b5 | 169 | // Main function start. |
MarijkeZondag | 8:895d941a5910 | 170 | |
vsluiter | 0:c8f15874531b | 171 | int main() |
MarijkeZondag | 3:a3ad58758696 | 172 | { |
MarijkeZondag | 25:c719346df3cd | 173 | pc.baud(115200); |
MarijkeZondag | 25:c719346df3cd | 174 | pc.printf("hello\n\r"); |
MarijkeZondag | 25:c719346df3cd | 175 | pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz |
MarijkeZondag | 25:c719346df3cd | 176 | |
MarijkeZondag | 25:c719346df3cd | 177 | encoder_tick.attach(&encoder_count,T); |
MarijkeZondag | 25:c719346df3cd | 178 | engine_tick.attach(&engine_control,T); |
MarijkeZondag | 25:c719346df3cd | 179 | //HIDScope ticker toevoegen |
MarijkeZondag | 25:c719346df3cd | 180 | |
MarijkeZondag | 25:c719346df3cd | 181 | // DEFININING VARIABLES |
MarijkeZondag | 25:c719346df3cd | 182 | |
MarijkeZondag | 25:c719346df3cd | 183 | //float reference; |
MarijkeZondag | 25:c719346df3cd | 184 | float foo[17]; |
MarijkeZondag | 25:c719346df3cd | 185 | float length = 9.0f; |
MarijkeZondag | 25:c719346df3cd | 186 | int a; |
MarijkeZondag | 25:c719346df3cd | 187 | int q; |
MarijkeZondag | 25:c719346df3cd | 188 | |
MarijkeZondag | 25:c719346df3cd | 189 | while (true) |
MarijkeZondag | 25:c719346df3cd | 190 | { |
MarijkeZondag | 6:f4bbb73f3989 | 191 | |
MarijkeZondag | 25:c719346df3cd | 192 | // EXECTURION IN MAIN |
MarijkeZondag | 25:c719346df3cd | 193 | // SIN ALS INPUT GEVEN |
MarijkeZondag | 25:c719346df3cd | 194 | if (button2 == false) |
MarijkeZondag | 25:c719346df3cd | 195 | { |
MarijkeZondag | 25:c719346df3cd | 196 | wait (0.2f); |
MarijkeZondag | 25:c719346df3cd | 197 | |
MarijkeZondag | 25:c719346df3cd | 198 | for (q=0; q<4; q++) |
MarijkeZondag | 25:c719346df3cd | 199 | { |
MarijkeZondag | 25:c719346df3cd | 200 | float b = 8.0f; |
MarijkeZondag | 25:c719346df3cd | 201 | |
MarijkeZondag | 25:c719346df3cd | 202 | for (a=0; a<10; a++) |
MarijkeZondag | 25:c719346df3cd | 203 | { |
MarijkeZondag | 25:c719346df3cd | 204 | foo[a] = 3.14f/length*(a); |
MarijkeZondag | 25:c719346df3cd | 205 | //printf("%f\n\r", foo[a]); |
MarijkeZondag | 25:c719346df3cd | 206 | reference_rotation = foo[a]; |
MarijkeZondag | 25:c719346df3cd | 207 | wait(0.2f); |
MarijkeZondag | 25:c719346df3cd | 208 | } |
MarijkeZondag | 25:c719346df3cd | 209 | |
MarijkeZondag | 25:c719346df3cd | 210 | for(a=10; a<18; a++) |
MarijkeZondag | 25:c719346df3cd | 211 | { |
MarijkeZondag | 25:c719346df3cd | 212 | foo[a] = 3.14f/length*b; |
MarijkeZondag | 25:c719346df3cd | 213 | //printf("%f\n\r", foo[a]); |
MarijkeZondag | 25:c719346df3cd | 214 | reference_rotation = foo[a]; |
MarijkeZondag | 25:c719346df3cd | 215 | b = b-1.0f; |
MarijkeZondag | 25:c719346df3cd | 216 | wait(0.2f); |
MarijkeZondag | 25:c719346df3cd | 217 | } |
MarijkeZondag | 25:c719346df3cd | 218 | } |
MarijkeZondag | 25:c719346df3cd | 219 | |
MarijkeZondag | 25:c719346df3cd | 220 | reference_rotation = 0; |
MarijkeZondag | 7:f32005d13749 | 221 | |
MarijkeZondag | 25:c719346df3cd | 222 | } |
MarijkeZondag | 24:b9dd6cf5c366 | 223 | |
vsluiter | 0:c8f15874531b | 224 | } |
MarijkeZondag | 25:c719346df3cd | 225 | } |