Diff: main.cpp

Revision:

0:9d0c764b4ae6

Child:

1:3af988298ce8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 14 18:33:20 2015 +0000
@@ -0,0 +1,47 @@
+# include "mbed.h"
+# include "QEI.h"
+# include "HIDScope.h"
+
+// Encoder definieren
+QEI Encoder1(D12,D13,NC,32); // Je gebruikt hierbij X2 encoding
+QEI Encoder2(D14,D15,NC,32); // Je gebruikt hierbij x2 encoding
+
+// Aantal pulses/deg
+const double P = 4200/360;
+const double L1 = 220.4;
+const double L2 = 130;
+
+
+double C1;
+double C2;
+double Q1;
+double Q2;
+double X;
+double Y;
+
+// HIDScope definieren
+HIDScope scope (2);
+Ticker Kinematicatimer;
+
+// Code voor de X- en Y-waarden
+void Kinematica ()
+{
+    C1 = Encoder1.getPulses();  //Aantal pulsen gemeten motor 1
+    C2 = Encoder2.getPulses();  //Aantal pulsen gemeten motor 2
+    Q1 = C1/P;  // Hoek van motor 1
+    Q2 = C2/P;  // Hoek van motor 2
+    X = cos (Q1) * L1 + cos (Q1+Q2) * L2; //X waarde van einde arm 2
+    Y = sin (Q1) * L1 + sin (Q1+Q2) * L2; //Y waarde van einde arm 2
+
+
+    scope.set (0, X);
+    scope.set (1, Y);
+    scope.send ();
+}
+
+int main ()
+{
+    Kinematicatimer.attach_us(&Kinematica, 1e4);
+    while(true) {}
+}
+