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@2:7993edc4dba6, 2015-10-23 (annotated)

Committer:
Thornlady
Date:
Fri Oct 23 08:15:35 2015 +0000
Revision:
2:7993edc4dba6
Parent:
1:3af988298ce8 
& weggehaald in int main, beschrijving toegevoegd van de doubles;

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 2:7993edc4dba6 10 const double P = 4200/360; //pulses/degree
Thornlady 2:7993edc4dba6 11 const double L1 = 220.4; //lengte arm 1
Thornlady 2:7993edc4dba6 12 const double L2 = 130; //lengte arm 2
Thornlady 2:7993edc4dba6 13 const double R = 2.166; //ratio van de tandwielen op de arm
Thornlady 0:9d0c764b4ae6 14
Thornlady 0:9d0c764b4ae6 15
Thornlady 2:7993edc4dba6 16 double C1; //aantal pulses motor 1
Thornlady 2:7993edc4dba6 17 double C2; //aantal pulses motor 2
Thornlady 2:7993edc4dba6 18 double Q1; //hoek motor 1
Thornlady 2:7993edc4dba6 19 double Q2; //hoek motor 2
Thornlady 2:7993edc4dba6 20 double X;
Thornlady 0:9d0c764b4ae6 21 double Y;
Thornlady 0:9d0c764b4ae6 22
Thornlady 0:9d0c764b4ae6 23 // HIDScope definieren
Thornlady 0:9d0c764b4ae6 24 HIDScope scope (2);
Thornlady 0:9d0c764b4ae6 25 Ticker Kinematicatimer;
Thornlady 0:9d0c764b4ae6 26
Thornlady 0:9d0c764b4ae6 27 // Code voor de X- en Y-waarden
Thornlady 0:9d0c764b4ae6 28 void Kinematica ()
Thornlady 0:9d0c764b4ae6 29 {
Thornlady 0:9d0c764b4ae6 30 C1 = Encoder1.getPulses(); //Aantal pulsen gemeten motor 1
Thornlady 0:9d0c764b4ae6 31 C2 = Encoder2.getPulses(); //Aantal pulsen gemeten motor 2
Thornlady 0:9d0c764b4ae6 32 Q1 = C1/P; // Hoek van motor 1
Thornlady 1:3af988298ce8 33 Q2 = R*C2/P; // Hoek van motor 2, met ratio van de tandwielen bovenop
Thornlady 2:7993edc4dba6 34 X = cos (Q1) * L1 + cos (Q1+Q2) * L2; //X waarde van einde arm 2
Thornlady 2:7993edc4dba6 35 Y = sin (Q1) * L1 + sin (Q1+Q2) * L2; //Y waarde van einde arm 2
Thornlady 0:9d0c764b4ae6 36
Thornlady 0:9d0c764b4ae6 37
Thornlady 0:9d0c764b4ae6 38 scope.set (0, X);
Thornlady 0:9d0c764b4ae6 39 scope.set (1, Y);
Thornlady 0:9d0c764b4ae6 40 scope.send ();
Thornlady 0:9d0c764b4ae6 41 }
Thornlady 0:9d0c764b4ae6 42
Thornlady 0:9d0c764b4ae6 43 int main ()
Thornlady 0:9d0c764b4ae6 44 {
Thornlady 2:7993edc4dba6 45 Kinematicatimer.attach_us(Kinematica, 1e4);
Thornlady 0:9d0c764b4ae6 46 while(true) {}
Thornlady 0:9d0c764b4ae6 47 }
Thornlady 0:9d0c764b4ae6 48