![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
pid afstellen
Diff: main.cpp
- Revision:
- 1:6a1ffe0cb8bd
- Parent:
- 0:3ca246e60129
diff -r 3ca246e60129 -r 6a1ffe0cb8bd main.cpp --- a/main.cpp Tue Oct 31 13:37:28 2017 +0000 +++ b/main.cpp Tue Oct 31 14:26:33 2017 +0000 @@ -5,16 +5,81 @@ Serial pc(USBTX, USBRX); //Serial PC connectie -QEI encoder1(D13, D12, NC, 32); //encoder instellen QEI encoder2(D11, D10, NC, 32); -DigitalOut motor1DirectionPin(D4); //Motorrichting op D4 (connected op het bord) -PwmOut motor1MagnitudePin(D5); //Motorkracht op D5 (connected op het bord) + DigitalOut motor2DirectionPin(D7); //Motorrichting op D4 (connected op het bord) PwmOut motor2MagnitudePin(D6); //Motorkracht op D5 (connected op het bord) -Ticker controller; //toets sample tijd +Ticker qwe; //toets ticker +Ticker besturing; +double fs = 1000; // sample frequentie +double ts = 0.001; +char key; +int pulses; +double ref = 0; + + +// PID variablen +double Kp = 0.001; +double Ki = 0; +double PI; + +double error1; +double error2 = 0; +double error_I = 0; +double error_I2= 0; + + + +void toetsen() +{ + if(pc.readable()==true) + { key = pc.getc(); + if (key=='w') + { + ref=25; //reference wordt 500 pulses + } + else if(key=='s') + { + ref=-25; //reference wordt 0 pulses + } + } +} + +void controller() +{ +pulses = encoder2.getPulses(); +error1 = ref - pulses; +error_I = error_I2 + ts*((error1 - error2)/2); + +PI = Kp*error1 + Ki*(error_I); + +error2 = error1; +error_I2 = error_I; + + //Motor control + if (PI<0 ) + { + motor2DirectionPin = 0; + motor2MagnitudePin = fabs(PI); + + } + else if (PI>0) + { + motor2DirectionPin = 1; + motor2MagnitudePin = fabs(PI); + } +} + + int main() { + pc.baud(115200); + qwe.attach_us(&toetsen, 100000); //ticker voor toetsen aflezen + besturing.attach_us(&controller, 1000); //ticker voor PI controller + + while(true) { + } } \ No newline at end of file