![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
encoder
Dependencies: MODSERIAL mbed HIDScope biquadFilter
Fork of Project_script by
main.cpp@24:b9dd6cf5c366, 2018-10-30 (annotated)
- Committer:
- MarijkeZondag
- Date:
- Tue Oct 30 15:02:26 2018 +0000
- Revision:
- 24:b9dd6cf5c366
- Parent:
- 23:c027e5c57cc8
- Child:
- 25:c719346df3cd
PID input, zonder motoraansturing
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" |
vsluiter | 0:c8f15874531b | 4 | |
MarijkeZondag | 9:c722418997b5 | 5 | AnalogIn potmetervalue1 (A1); |
MarijkeZondag | 9:c722418997b5 | 6 | AnalogIn potmetervalue2 (A2); |
MarijkeZondag | 23:c027e5c57cc8 | 7 | |
MarijkeZondag | 9:c722418997b5 | 8 | DigitalIn button2 (D10); //Let op, is deze niet bezet? |
MarijkeZondag | 23:c027e5c57cc8 | 9 | |
MarijkeZondag | 9:c722418997b5 | 10 | InterruptIn encoderA (D9); |
MarijkeZondag | 9:c722418997b5 | 11 | InterruptIn encoderB (D8); |
MarijkeZondag | 6:f4bbb73f3989 | 12 | |
MarijkeZondag | 23:c027e5c57cc8 | 13 | DigitalOut directionpin1 (D7); |
MarijkeZondag | 23:c027e5c57cc8 | 14 | DigitalOut directionpin2 (D4); |
MarijkeZondag | 23:c027e5c57cc8 | 15 | PwmOut pwmpin1 (D6); |
MarijkeZondag | 23:c027e5c57cc8 | 16 | PwmOut pwmpin2 (D5); |
MarijkeZondag | 9:c722418997b5 | 17 | |
MarijkeZondag | 6:f4bbb73f3989 | 18 | |
vsluiter | 0:c8f15874531b | 19 | MODSERIAL pc(USBTX, USBRX); |
vsluiter | 0:c8f15874531b | 20 | |
MarijkeZondag | 9:c722418997b5 | 21 | |
MarijkeZondag | 9:c722418997b5 | 22 | //Global variables |
MarijkeZondag | 24:b9dd6cf5c366 | 23 | int encoder = 0; //Start value encoder |
MarijkeZondag | 24:b9dd6cf5c366 | 24 | int T = 0.002f; //Ticker period |
MarijkeZondag | 24:b9dd6cf5c366 | 25 | |
MarijkeZondag | 24:b9dd6cf5c366 | 26 | double Kp = 17.5; |
MarijkeZondag | 24:b9dd6cf5c366 | 27 | double Ki = 1.02; |
MarijkeZondag | 24:b9dd6cf5c366 | 28 | double Kd = 23.2; |
MarijkeZondag | 24:b9dd6cf5c366 | 29 | |
MarijkeZondag | 24:b9dd6cf5c366 | 30 | double err = 0; |
MarijkeZondag | 24:b9dd6cf5c366 | 31 | |
MarijkeZondag | 24:b9dd6cf5c366 | 32 | //Tickers |
MarijkeZondag | 24:b9dd6cf5c366 | 33 | Ticker function_tick; |
MarijkeZondag | 8:895d941a5910 | 34 | |
MarijkeZondag | 9:c722418997b5 | 35 | |
MarijkeZondag | 9:c722418997b5 | 36 | //Functions |
MarijkeZondag | 8:895d941a5910 | 37 | void encoderA_rise() |
MarijkeZondag | 8:895d941a5910 | 38 | { |
MarijkeZondag | 8:895d941a5910 | 39 | if(encoderB==false) |
MarijkeZondag | 8:895d941a5910 | 40 | { |
MarijkeZondag | 8:895d941a5910 | 41 | encoder++; |
MarijkeZondag | 8:895d941a5910 | 42 | } |
MarijkeZondag | 8:895d941a5910 | 43 | else |
MarijkeZondag | 8:895d941a5910 | 44 | { |
MarijkeZondag | 8:895d941a5910 | 45 | encoder--; |
MarijkeZondag | 8:895d941a5910 | 46 | } |
MarijkeZondag | 8:895d941a5910 | 47 | } |
MarijkeZondag | 8:895d941a5910 | 48 | |
MarijkeZondag | 8:895d941a5910 | 49 | void encoderA_fall() |
MarijkeZondag | 8:895d941a5910 | 50 | { |
MarijkeZondag | 8:895d941a5910 | 51 | if(encoderB==true) |
MarijkeZondag | 8:895d941a5910 | 52 | { |
MarijkeZondag | 8:895d941a5910 | 53 | encoder++; |
MarijkeZondag | 8:895d941a5910 | 54 | } |
MarijkeZondag | 8:895d941a5910 | 55 | else |
MarijkeZondag | 8:895d941a5910 | 56 | { |
MarijkeZondag | 8:895d941a5910 | 57 | encoder--; |
MarijkeZondag | 8:895d941a5910 | 58 | } |
MarijkeZondag | 8:895d941a5910 | 59 | } |
MarijkeZondag | 8:895d941a5910 | 60 | |
MarijkeZondag | 8:895d941a5910 | 61 | void encoderB_rise() |
MarijkeZondag | 8:895d941a5910 | 62 | { |
MarijkeZondag | 8:895d941a5910 | 63 | if(encoderA==true) |
MarijkeZondag | 8:895d941a5910 | 64 | { |
MarijkeZondag | 8:895d941a5910 | 65 | encoder++; |
MarijkeZondag | 8:895d941a5910 | 66 | } |
MarijkeZondag | 8:895d941a5910 | 67 | else |
MarijkeZondag | 8:895d941a5910 | 68 | { |
MarijkeZondag | 8:895d941a5910 | 69 | encoder--; |
MarijkeZondag | 8:895d941a5910 | 70 | } |
MarijkeZondag | 8:895d941a5910 | 71 | } |
MarijkeZondag | 8:895d941a5910 | 72 | |
MarijkeZondag | 8:895d941a5910 | 73 | void encoderB_fall() |
MarijkeZondag | 8:895d941a5910 | 74 | { |
MarijkeZondag | 8:895d941a5910 | 75 | if(encoderA==false) |
MarijkeZondag | 8:895d941a5910 | 76 | { |
MarijkeZondag | 8:895d941a5910 | 77 | encoder++; |
MarijkeZondag | 8:895d941a5910 | 78 | } |
MarijkeZondag | 8:895d941a5910 | 79 | else |
MarijkeZondag | 8:895d941a5910 | 80 | { |
MarijkeZondag | 8:895d941a5910 | 81 | encoder--; |
MarijkeZondag | 8:895d941a5910 | 82 | } |
MarijkeZondag | 8:895d941a5910 | 83 | } |
MarijkeZondag | 8:895d941a5910 | 84 | |
MarijkeZondag | 24:b9dd6cf5c366 | 85 | void encoder_count() |
MarijkeZondag | 24:b9dd6cf5c366 | 86 | { |
MarijkeZondag | 24:b9dd6cf5c366 | 87 | encoderA.rise(&encoderA_rise); |
MarijkeZondag | 24:b9dd6cf5c366 | 88 | encoderA.fall(&encoderA_fall); |
MarijkeZondag | 24:b9dd6cf5c366 | 89 | encoderB.rise(&encoderB_rise); |
MarijkeZondag | 24:b9dd6cf5c366 | 90 | encoderB.fall(&encoderB_fall); |
MarijkeZondag | 24:b9dd6cf5c366 | 91 | } |
MarijkeZondag | 24:b9dd6cf5c366 | 92 | |
MarijkeZondag | 24:b9dd6cf5c366 | 93 | double PID_controller() |
MarijkeZondag | 24:b9dd6cf5c366 | 94 | { |
MarijkeZondag | 24:b9dd6cf5c366 | 95 | static double err_integral = 0; |
MarijkeZondag | 24:b9dd6cf5c366 | 96 | static double err_prev = err; // initialization with this value only done once! |
MarijkeZondag | 24:b9dd6cf5c366 | 97 | |
MarijkeZondag | 24:b9dd6cf5c366 | 98 | static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); |
MarijkeZondag | 24:b9dd6cf5c366 | 99 | |
MarijkeZondag | 24:b9dd6cf5c366 | 100 | // Proportional part: |
MarijkeZondag | 24:b9dd6cf5c366 | 101 | double u_k = Kp * err; |
MarijkeZondag | 24:b9dd6cf5c366 | 102 | |
MarijkeZondag | 24:b9dd6cf5c366 | 103 | // Integral part |
MarijkeZondag | 24:b9dd6cf5c366 | 104 | err_integral = err_integral + err * T; |
MarijkeZondag | 24:b9dd6cf5c366 | 105 | double u_i = Ki * err_integral; |
MarijkeZondag | 24:b9dd6cf5c366 | 106 | |
MarijkeZondag | 24:b9dd6cf5c366 | 107 | // Derivative part |
MarijkeZondag | 24:b9dd6cf5c366 | 108 | double err_derivative = (err - err_prev)/T; |
MarijkeZondag | 24:b9dd6cf5c366 | 109 | double filtered_err_derivative = LowPassFilter.step(err_derivative); |
MarijkeZondag | 24:b9dd6cf5c366 | 110 | double u_d = Kd * filtered_err_derivative; |
MarijkeZondag | 24:b9dd6cf5c366 | 111 | err_prev = err; |
MarijkeZondag | 24:b9dd6cf5c366 | 112 | |
MarijkeZondag | 24:b9dd6cf5c366 | 113 | // Sum all parts and return it |
MarijkeZondag | 24:b9dd6cf5c366 | 114 | return u_k + u_i + u_d; |
MarijkeZondag | 24:b9dd6cf5c366 | 115 | } |
MarijkeZondag | 24:b9dd6cf5c366 | 116 | |
MarijkeZondag | 24:b9dd6cf5c366 | 117 | void call_all_functions() |
MarijkeZondag | 24:b9dd6cf5c366 | 118 | { |
MarijkeZondag | 24:b9dd6cf5c366 | 119 | PID_controller(); |
MarijkeZondag | 24:b9dd6cf5c366 | 120 | encoder_count(); |
MarijkeZondag | 24:b9dd6cf5c366 | 121 | } |
MarijkeZondag | 8:895d941a5910 | 122 | |
MarijkeZondag | 9:c722418997b5 | 123 | // Main function start. |
MarijkeZondag | 8:895d941a5910 | 124 | |
vsluiter | 0:c8f15874531b | 125 | int main() |
MarijkeZondag | 3:a3ad58758696 | 126 | { |
vsluiter | 0:c8f15874531b | 127 | pc.baud(115200); |
MarijkeZondag | 6:f4bbb73f3989 | 128 | pc.printf("hello\n\r"); |
MarijkeZondag | 6:f4bbb73f3989 | 129 | pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz |
MarijkeZondag | 6:f4bbb73f3989 | 130 | |
MarijkeZondag | 24:b9dd6cf5c366 | 131 | function_tick.attach(&call_all_functions,T); |
MarijkeZondag | 24:b9dd6cf5c366 | 132 | |
vsluiter | 0:c8f15874531b | 133 | |
MarijkeZondag | 2:2f4444f1504d | 134 | while (true) |
MarijkeZondag | 2:2f4444f1504d | 135 | { |
MarijkeZondag | 7:f32005d13749 | 136 | |
MarijkeZondag | 24:b9dd6cf5c366 | 137 | |
MarijkeZondag | 9:c722418997b5 | 138 | |
vsluiter | 0:c8f15874531b | 139 | } |
vsluiter | 0:c8f15874531b | 140 | } |
MarijkeZondag | 4:fbea05c8fe2b | 141 |