Jorn-Jan van de Beld / Mbed 2 deprecated PID_pulses

PID control with use of pulses of encoder

Dependencies:   QEI mbed

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?

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