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?
User | Revision | Line number | New 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 |