Kirsten van Dorenvanck / Mbed 2 deprecated Kinematica

Kinematica robot M9 Gr 13. Geeft de x en y waarden op hidscope weer in 2 scopes. De waarden zijn berekend met het aantal pulses van de encoders van beide motoren.

Dependencies:   HIDScope QEI mbed

Diff: main.cpp

Revision:
1:3af988298ce8
Parent:
0:9d0c764b4ae6
Child:
2:7993edc4dba6
--- a/main.cpp	Wed Oct 14 18:33:20 2015 +0000
+++ b/main.cpp	Fri Oct 16 08:50:19 2015 +0000
@@ -10,6 +10,7 @@
 const double P = 4200/360;
 const double L1 = 220.4;
 const double L2 = 130;
+const double R = 2.166; //ratio van de tandwielen op de arm
 
 
 double C1;
@@ -29,9 +30,9 @@
     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
+    Q2 = R*C2/P;  // Hoek van motor 2, met ratio van de tandwielen bovenop
+    X = cos (Q1) * L1 + cos (Q1+Q2) * L2; //X waarde van einde arm 2    RATIO VAN DE TANDWIELEN NOG NIET ERBIJ 
+    Y = sin (Q1) * L1 + sin (Q1+Q2) * L2; //Y waarde van einde arm 2    RATIO VAN DE TANDWIELEN NOG NIET ERBIJ!!!
 
 
     scope.set (0, X);