Jorn-Jan van de Beld
/
PID_pulses
PID control with use of pulses of encoder
Diff: main.cpp
- Revision:
- 1:8c585244c1b0
- Parent:
- 0:cf438428bc75
diff -r cf438428bc75 -r 8c585244c1b0 main.cpp --- a/main.cpp Fri Oct 20 12:37:13 2017 +0000 +++ b/main.cpp Fri Oct 20 13:45:35 2017 +0000 @@ -4,33 +4,71 @@ #include "math.h" +Serial pc(USBTX, USBRX); //Serial PC connectie QEI encoder(D13, D12, NC, 32); //encoder instellen DigitalOut motor1DirectionPin(D4); //Motorrichting op D4 (connected op het bord) PwmOut motor1MagnitudePin(D5); //Motorkracht op D5 (connected op het bord) Ticker controller; //toets sample tijd +double fs = 10000; // sample frequentie +double ts = 1/fs; +// variablen voor void int pulses; -int ref = 400; +int ref; int er; -float Kp=0.0008; +float Kp=0.0005; float P; +float D; +float Dif; +float Kd=0.00012; +float PID; +int er2=0; void PD() { +char key; + if(pc.readable()==true) + { key = pc.getc(); + if (key=='q') + { + ref=-500; //reference wordt 500 pulses + } + else if(key=='w') + { + ref=0; + } + else if(key=='e') + { + ref=500; //reference wordt 0 pulses + } + } +//error bepalen pulses=encoder.getPulses(); er=ref-pulses; + +//PID +//Proportional part P = Kp*er; -if (P<0) -{ -motor1MagnitudePin = fabs(P); -motor1DirectionPin = 1; -} -else if (P>0) -{ -motor1MagnitudePin = fabs(P); -motor1DirectionPin = 0; -} +//Differential part +Dif=(er2-er)/ts; +D=Kd*Dif; + +//PID sum +PID=P+D; +er2=er; + + //Motor control + if (P<0) + { + motor1MagnitudePin = fabs(P); + motor1DirectionPin = 1; + } + else if (P>0) + { + motor1MagnitudePin = fabs(P); + motor1DirectionPin = 0; + } }