Jorn-Jan van de Beld / Mbed 2 deprecated PID_testen

pid afstellen

Dependencies:   QEI mbed

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?

UserRevisionLine numberNew 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 }