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@1:3af988298ce8, 2015-10-16 (annotated)

Committer:
Thornlady
Date:
Fri Oct 16 08:50:19 2015 +0000
Revision:
1:3af988298ce8
Parent:
0:9d0c764b4ae6
Child:
2:7993edc4dba6
Ratio van de tandwielen toegevoegd bij de hoek van de 2e arm. In de formule lijkt het alsof er 2.166x zo veel pulses zijn, dit komt door de ratio.

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 1:3af988298ce8 13 const double R = 2.166; //ratio van de tandwielen op de arm
Thornlady 0:9d0c764b4ae6 14
Thornlady 0:9d0c764b4ae6 15
Thornlady 0:9d0c764b4ae6 16 double C1;
Thornlady 0:9d0c764b4ae6 17 double C2;
Thornlady 0:9d0c764b4ae6 18 double Q1;
Thornlady 0:9d0c764b4ae6 19 double Q2;
Thornlady 0:9d0c764b4ae6 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 1:3af988298ce8 34 X = cos (Q1) * L1 + cos (Q1+Q2) * L2; //X waarde van einde arm 2 RATIO VAN DE TANDWIELEN NOG NIET ERBIJ
Thornlady 1:3af988298ce8 35 Y = sin (Q1) * L1 + sin (Q1+Q2) * L2; //Y waarde van einde arm 2 RATIO VAN DE TANDWIELEN NOG NIET ERBIJ!!!
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 0:9d0c764b4ae6 45 Kinematicatimer.attach_us(&Kinematica, 1e4);
Thornlady 0:9d0c764b4ae6 46 while(true) {}
Thornlady 0:9d0c764b4ae6 47 }
Thornlady 0:9d0c764b4ae6 48