Jorn-Jan van de Beld
/
PID_testen
pid afstellen
main.cpp@1:6a1ffe0cb8bd, 2017-10-31 (annotated)
- Committer:
- JornJan
- Date:
- Tue Oct 31 14:26:33 2017 +0000
- Revision:
- 1:6a1ffe0cb8bd
- Parent:
- 0:3ca246e60129
JJ ervandoor;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JornJan | 0:3ca246e60129 | 1 | #include "mbed.h" |
JornJan | 0:3ca246e60129 | 2 | #include "Serial.h" |
JornJan | 0:3ca246e60129 | 3 | #include "QEI.h" |
JornJan | 0:3ca246e60129 | 4 | #include "math.h" |
JornJan | 0:3ca246e60129 | 5 | |
JornJan | 0:3ca246e60129 | 6 | |
JornJan | 0:3ca246e60129 | 7 | Serial pc(USBTX, USBRX); //Serial PC connectie |
JornJan | 0:3ca246e60129 | 8 | QEI encoder2(D11, D10, NC, 32); |
JornJan | 0:3ca246e60129 | 9 | |
JornJan | 1:6a1ffe0cb8bd | 10 | |
JornJan | 0:3ca246e60129 | 11 | DigitalOut motor2DirectionPin(D7); //Motorrichting op D4 (connected op het bord) |
JornJan | 0:3ca246e60129 | 12 | PwmOut motor2MagnitudePin(D6); //Motorkracht op D5 (connected op het bord) |
JornJan | 0:3ca246e60129 | 13 | |
JornJan | 1:6a1ffe0cb8bd | 14 | Ticker qwe; //toets ticker |
JornJan | 1:6a1ffe0cb8bd | 15 | Ticker besturing; |
JornJan | 1:6a1ffe0cb8bd | 16 | double fs = 1000; // sample frequentie |
JornJan | 1:6a1ffe0cb8bd | 17 | double ts = 0.001; |
JornJan | 1:6a1ffe0cb8bd | 18 | char key; |
JornJan | 1:6a1ffe0cb8bd | 19 | int pulses; |
JornJan | 1:6a1ffe0cb8bd | 20 | double ref = 0; |
JornJan | 1:6a1ffe0cb8bd | 21 | |
JornJan | 1:6a1ffe0cb8bd | 22 | |
JornJan | 1:6a1ffe0cb8bd | 23 | // PID variablen |
JornJan | 1:6a1ffe0cb8bd | 24 | double Kp = 0.001; |
JornJan | 1:6a1ffe0cb8bd | 25 | double Ki = 0; |
JornJan | 1:6a1ffe0cb8bd | 26 | double PI; |
JornJan | 1:6a1ffe0cb8bd | 27 | |
JornJan | 1:6a1ffe0cb8bd | 28 | double error1; |
JornJan | 1:6a1ffe0cb8bd | 29 | double error2 = 0; |
JornJan | 1:6a1ffe0cb8bd | 30 | double error_I = 0; |
JornJan | 1:6a1ffe0cb8bd | 31 | double error_I2= 0; |
JornJan | 1:6a1ffe0cb8bd | 32 | |
JornJan | 1:6a1ffe0cb8bd | 33 | |
JornJan | 1:6a1ffe0cb8bd | 34 | |
JornJan | 1:6a1ffe0cb8bd | 35 | void toetsen() |
JornJan | 1:6a1ffe0cb8bd | 36 | { |
JornJan | 1:6a1ffe0cb8bd | 37 | if(pc.readable()==true) |
JornJan | 1:6a1ffe0cb8bd | 38 | { key = pc.getc(); |
JornJan | 1:6a1ffe0cb8bd | 39 | if (key=='w') |
JornJan | 1:6a1ffe0cb8bd | 40 | { |
JornJan | 1:6a1ffe0cb8bd | 41 | ref=25; //reference wordt 500 pulses |
JornJan | 1:6a1ffe0cb8bd | 42 | } |
JornJan | 1:6a1ffe0cb8bd | 43 | else if(key=='s') |
JornJan | 1:6a1ffe0cb8bd | 44 | { |
JornJan | 1:6a1ffe0cb8bd | 45 | ref=-25; //reference wordt 0 pulses |
JornJan | 1:6a1ffe0cb8bd | 46 | } |
JornJan | 1:6a1ffe0cb8bd | 47 | } |
JornJan | 1:6a1ffe0cb8bd | 48 | } |
JornJan | 1:6a1ffe0cb8bd | 49 | |
JornJan | 1:6a1ffe0cb8bd | 50 | void controller() |
JornJan | 1:6a1ffe0cb8bd | 51 | { |
JornJan | 1:6a1ffe0cb8bd | 52 | pulses = encoder2.getPulses(); |
JornJan | 1:6a1ffe0cb8bd | 53 | error1 = ref - pulses; |
JornJan | 1:6a1ffe0cb8bd | 54 | error_I = error_I2 + ts*((error1 - error2)/2); |
JornJan | 1:6a1ffe0cb8bd | 55 | |
JornJan | 1:6a1ffe0cb8bd | 56 | PI = Kp*error1 + Ki*(error_I); |
JornJan | 1:6a1ffe0cb8bd | 57 | |
JornJan | 1:6a1ffe0cb8bd | 58 | error2 = error1; |
JornJan | 1:6a1ffe0cb8bd | 59 | error_I2 = error_I; |
JornJan | 1:6a1ffe0cb8bd | 60 | |
JornJan | 1:6a1ffe0cb8bd | 61 | //Motor control |
JornJan | 1:6a1ffe0cb8bd | 62 | if (PI<0 ) |
JornJan | 1:6a1ffe0cb8bd | 63 | { |
JornJan | 1:6a1ffe0cb8bd | 64 | motor2DirectionPin = 0; |
JornJan | 1:6a1ffe0cb8bd | 65 | motor2MagnitudePin = fabs(PI); |
JornJan | 1:6a1ffe0cb8bd | 66 | |
JornJan | 1:6a1ffe0cb8bd | 67 | } |
JornJan | 1:6a1ffe0cb8bd | 68 | else if (PI>0) |
JornJan | 1:6a1ffe0cb8bd | 69 | { |
JornJan | 1:6a1ffe0cb8bd | 70 | motor2DirectionPin = 1; |
JornJan | 1:6a1ffe0cb8bd | 71 | motor2MagnitudePin = fabs(PI); |
JornJan | 1:6a1ffe0cb8bd | 72 | } |
JornJan | 1:6a1ffe0cb8bd | 73 | } |
JornJan | 1:6a1ffe0cb8bd | 74 | |
JornJan | 1:6a1ffe0cb8bd | 75 | |
JornJan | 0:3ca246e60129 | 76 | |
JornJan | 0:3ca246e60129 | 77 | int main() |
JornJan | 0:3ca246e60129 | 78 | { |
JornJan | 1:6a1ffe0cb8bd | 79 | pc.baud(115200); |
JornJan | 1:6a1ffe0cb8bd | 80 | qwe.attach_us(&toetsen, 100000); //ticker voor toetsen aflezen |
JornJan | 1:6a1ffe0cb8bd | 81 | besturing.attach_us(&controller, 1000); //ticker voor PI controller |
JornJan | 1:6a1ffe0cb8bd | 82 | |
JornJan | 1:6a1ffe0cb8bd | 83 | while(true) { |
JornJan | 1:6a1ffe0cb8bd | 84 | } |
JornJan | 0:3ca246e60129 | 85 | } |