Jorn-Jan van de Beld
/
PID_pulses
PID control with use of pulses of encoder
main.cpp@1:8c585244c1b0, 2017-10-20 (annotated)
- Committer:
- JornJan
- Date:
- Fri Oct 20 13:45:35 2017 +0000
- Revision:
- 1:8c585244c1b0
- Parent:
- 0:cf438428bc75
Derivative included;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JornJan | 0:cf438428bc75 | 1 | #include "mbed.h" |
JornJan | 0:cf438428bc75 | 2 | #include "Serial.h" |
JornJan | 0:cf438428bc75 | 3 | #include "QEI.h" |
JornJan | 0:cf438428bc75 | 4 | #include "math.h" |
JornJan | 0:cf438428bc75 | 5 | |
JornJan | 0:cf438428bc75 | 6 | |
JornJan | 1:8c585244c1b0 | 7 | Serial pc(USBTX, USBRX); //Serial PC connectie |
JornJan | 0:cf438428bc75 | 8 | QEI encoder(D13, D12, NC, 32); //encoder instellen |
JornJan | 0:cf438428bc75 | 9 | DigitalOut motor1DirectionPin(D4); //Motorrichting op D4 (connected op het bord) |
JornJan | 0:cf438428bc75 | 10 | PwmOut motor1MagnitudePin(D5); //Motorkracht op D5 (connected op het bord) |
JornJan | 0:cf438428bc75 | 11 | Ticker controller; //toets sample tijd |
JornJan | 1:8c585244c1b0 | 12 | double fs = 10000; // sample frequentie |
JornJan | 1:8c585244c1b0 | 13 | double ts = 1/fs; |
JornJan | 0:cf438428bc75 | 14 | |
JornJan | 1:8c585244c1b0 | 15 | // variablen voor void |
JornJan | 0:cf438428bc75 | 16 | int pulses; |
JornJan | 1:8c585244c1b0 | 17 | int ref; |
JornJan | 0:cf438428bc75 | 18 | int er; |
JornJan | 1:8c585244c1b0 | 19 | float Kp=0.0005; |
JornJan | 0:cf438428bc75 | 20 | float P; |
JornJan | 1:8c585244c1b0 | 21 | float D; |
JornJan | 1:8c585244c1b0 | 22 | float Dif; |
JornJan | 1:8c585244c1b0 | 23 | float Kd=0.00012; |
JornJan | 1:8c585244c1b0 | 24 | float PID; |
JornJan | 1:8c585244c1b0 | 25 | int er2=0; |
JornJan | 0:cf438428bc75 | 26 | |
JornJan | 0:cf438428bc75 | 27 | void PD() |
JornJan | 0:cf438428bc75 | 28 | { |
JornJan | 1:8c585244c1b0 | 29 | char key; |
JornJan | 1:8c585244c1b0 | 30 | if(pc.readable()==true) |
JornJan | 1:8c585244c1b0 | 31 | { key = pc.getc(); |
JornJan | 1:8c585244c1b0 | 32 | if (key=='q') |
JornJan | 1:8c585244c1b0 | 33 | { |
JornJan | 1:8c585244c1b0 | 34 | ref=-500; //reference wordt 500 pulses |
JornJan | 1:8c585244c1b0 | 35 | } |
JornJan | 1:8c585244c1b0 | 36 | else if(key=='w') |
JornJan | 1:8c585244c1b0 | 37 | { |
JornJan | 1:8c585244c1b0 | 38 | ref=0; |
JornJan | 1:8c585244c1b0 | 39 | } |
JornJan | 1:8c585244c1b0 | 40 | else if(key=='e') |
JornJan | 1:8c585244c1b0 | 41 | { |
JornJan | 1:8c585244c1b0 | 42 | ref=500; //reference wordt 0 pulses |
JornJan | 1:8c585244c1b0 | 43 | } |
JornJan | 1:8c585244c1b0 | 44 | } |
JornJan | 1:8c585244c1b0 | 45 | //error bepalen |
JornJan | 0:cf438428bc75 | 46 | pulses=encoder.getPulses(); |
JornJan | 0:cf438428bc75 | 47 | er=ref-pulses; |
JornJan | 1:8c585244c1b0 | 48 | |
JornJan | 1:8c585244c1b0 | 49 | //PID |
JornJan | 1:8c585244c1b0 | 50 | //Proportional part |
JornJan | 0:cf438428bc75 | 51 | P = Kp*er; |
JornJan | 0:cf438428bc75 | 52 | |
JornJan | 1:8c585244c1b0 | 53 | //Differential part |
JornJan | 1:8c585244c1b0 | 54 | Dif=(er2-er)/ts; |
JornJan | 1:8c585244c1b0 | 55 | D=Kd*Dif; |
JornJan | 1:8c585244c1b0 | 56 | |
JornJan | 1:8c585244c1b0 | 57 | //PID sum |
JornJan | 1:8c585244c1b0 | 58 | PID=P+D; |
JornJan | 1:8c585244c1b0 | 59 | er2=er; |
JornJan | 1:8c585244c1b0 | 60 | |
JornJan | 1:8c585244c1b0 | 61 | //Motor control |
JornJan | 1:8c585244c1b0 | 62 | if (P<0) |
JornJan | 1:8c585244c1b0 | 63 | { |
JornJan | 1:8c585244c1b0 | 64 | motor1MagnitudePin = fabs(P); |
JornJan | 1:8c585244c1b0 | 65 | motor1DirectionPin = 1; |
JornJan | 1:8c585244c1b0 | 66 | } |
JornJan | 1:8c585244c1b0 | 67 | else if (P>0) |
JornJan | 1:8c585244c1b0 | 68 | { |
JornJan | 1:8c585244c1b0 | 69 | motor1MagnitudePin = fabs(P); |
JornJan | 1:8c585244c1b0 | 70 | motor1DirectionPin = 0; |
JornJan | 1:8c585244c1b0 | 71 | } |
JornJan | 0:cf438428bc75 | 72 | |
JornJan | 0:cf438428bc75 | 73 | } |
JornJan | 0:cf438428bc75 | 74 | |
JornJan | 0:cf438428bc75 | 75 | int main() |
JornJan | 0:cf438428bc75 | 76 | { |
JornJan | 0:cf438428bc75 | 77 | controller.attach_us(&PD, 10000); |
JornJan | 0:cf438428bc75 | 78 | |
JornJan | 0:cf438428bc75 | 79 | while(true) |
JornJan | 0:cf438428bc75 | 80 | { |
JornJan | 0:cf438428bc75 | 81 | } |
JornJan | 0:cf438428bc75 | 82 | |
JornJan | 0:cf438428bc75 | 83 | |
JornJan | 0:cf438428bc75 | 84 | } |