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

main.cpp@0:9d0c764b4ae6, 2015-10-14 (annotated)

Committer:
Thornlady
Date:
Wed Oct 14 18:33:20 2015 +0000
Revision:
0:9d0c764b4ae6
Child:
1:3af988298ce8
Kinematica versie 1: simpele inverse kinematica toegepast, encoder pulses naar hoeken vertaald en toegepast in formule naar X- en Y-waarden. Deze worden naar Hidscope gestuurd.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Thornlady 0:9d0c764b4ae6 1 # include "mbed.h"
Thornlady 0:9d0c764b4ae6 2 # include "QEI.h"
Thornlady 0:9d0c764b4ae6 3 # include "HIDScope.h"
Thornlady 0:9d0c764b4ae6 4
Thornlady 0:9d0c764b4ae6 5 // Encoder definieren
Thornlady 0:9d0c764b4ae6 6 QEI Encoder1(D12,D13,NC,32); // Je gebruikt hierbij X2 encoding
Thornlady 0:9d0c764b4ae6 7 QEI Encoder2(D14,D15,NC,32); // Je gebruikt hierbij x2 encoding
Thornlady 0:9d0c764b4ae6 8
Thornlady 0:9d0c764b4ae6 9 // Aantal pulses/deg
Thornlady 0:9d0c764b4ae6 10 const double P = 4200/360;
Thornlady 0:9d0c764b4ae6 11 const double L1 = 220.4;
Thornlady 0:9d0c764b4ae6 12 const double L2 = 130;
Thornlady 0:9d0c764b4ae6 13
Thornlady 0:9d0c764b4ae6 14
Thornlady 0:9d0c764b4ae6 15 double C1;
Thornlady 0:9d0c764b4ae6 16 double C2;
Thornlady 0:9d0c764b4ae6 17 double Q1;
Thornlady 0:9d0c764b4ae6 18 double Q2;
Thornlady 0:9d0c764b4ae6 19 double X;
Thornlady 0:9d0c764b4ae6 20 double Y;
Thornlady 0:9d0c764b4ae6 21
Thornlady 0:9d0c764b4ae6 22 // HIDScope definieren
Thornlady 0:9d0c764b4ae6 23 HIDScope scope (2);
Thornlady 0:9d0c764b4ae6 24 Ticker Kinematicatimer;
Thornlady 0:9d0c764b4ae6 25
Thornlady 0:9d0c764b4ae6 26 // Code voor de X- en Y-waarden
Thornlady 0:9d0c764b4ae6 27 void Kinematica ()
Thornlady 0:9d0c764b4ae6 28 {
Thornlady 0:9d0c764b4ae6 29 C1 = Encoder1.getPulses(); //Aantal pulsen gemeten motor 1
Thornlady 0:9d0c764b4ae6 30 C2 = Encoder2.getPulses(); //Aantal pulsen gemeten motor 2
Thornlady 0:9d0c764b4ae6 31 Q1 = C1/P; // Hoek van motor 1
Thornlady 0:9d0c764b4ae6 32 Q2 = C2/P; // Hoek van motor 2
Thornlady 0:9d0c764b4ae6 33 X = cos (Q1) * L1 + cos (Q1+Q2) * L2; //X waarde van einde arm 2
Thornlady 0:9d0c764b4ae6 34 Y = sin (Q1) * L1 + sin (Q1+Q2) * L2; //Y waarde van einde arm 2
Thornlady 0:9d0c764b4ae6 35
Thornlady 0:9d0c764b4ae6 36
Thornlady 0:9d0c764b4ae6 37 scope.set (0, X);
Thornlady 0:9d0c764b4ae6 38 scope.set (1, Y);
Thornlady 0:9d0c764b4ae6 39 scope.send ();
Thornlady 0:9d0c764b4ae6 40 }
Thornlady 0:9d0c764b4ae6 41
Thornlady 0:9d0c764b4ae6 42 int main ()
Thornlady 0:9d0c764b4ae6 43 {
Thornlady 0:9d0c764b4ae6 44 Kinematicatimer.attach_us(&Kinematica, 1e4);
Thornlady 0:9d0c764b4ae6 45 while(true) {}
Thornlady 0:9d0c764b4ae6 46 }
Thornlady 0:9d0c764b4ae6 47