Diff: main.cpp

Revision:

1:6a1ffe0cb8bd

Parent:

0:3ca246e60129

--- 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