Diff: main.cpp

Revision:

0:cf438428bc75

Child:

1:8c585244c1b0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 20 12:37:13 2017 +0000
@@ -0,0 +1,46 @@
+#include "mbed.h"
+#include "Serial.h"
+#include "QEI.h"
+#include "math.h"
+
+
+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
+
+int pulses;
+int ref = 400;
+int er;
+float Kp=0.0008;
+float P;
+
+void PD()
+{
+pulses=encoder.getPulses();
+er=ref-pulses;
+P = Kp*er;
+
+if (P<0)
+{
+motor1MagnitudePin = fabs(P);
+motor1DirectionPin = 1;
+}
+else if (P>0)
+{
+motor1MagnitudePin = fabs(P);
+motor1DirectionPin = 0;
+}
+
+}
+
+int main()
+{
+controller.attach_us(&PD, 10000);
+    
+while(true)
+{
+}
+
+    
+}
\ No newline at end of file