Diff: main.cpp

Revision:

1:8c585244c1b0

Parent:

0:cf438428bc75

--- 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;
+    }
 
 }