Jorn-Jan van de Beld / Mbed 2 deprecated kin_PD_project

begin van episch avontuur

Dependencies:   QEI mbed

Diff: main.cpp

Revision:
4:95bf97b44237
Parent:
3:329acce2487c
Child:
5:21e846902e3e
--- a/main.cpp	Tue Oct 31 13:26:52 2017 +0000
+++ b/main.cpp	Tue Oct 31 15:41:40 2017 +0000
@@ -163,6 +163,31 @@
         
         
     }
+    
+void controller(double q1ref, double q2ref) //HIER WAS JE BEZIG@!#!@#!@#@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
+{
+pulses1 = q1_enc.getPulses();
+error1_1 = ref1 - pulses1;
+error1_I = error1_I2 + ts*((error1_1 - error2_1)/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);
+    }
+}
 
 void aansturing()
     {