ticker toegevoegd en juiste kinematica (met frame 1 en 2 op verschillende x positie) toegepast
Fork of Project_Kinematics by
TestRKI.cpp@2:8eca67634cba, 2018-10-30 (annotated)
- Committer:
- Marle
- Date:
- Tue Oct 30 18:21:31 2018 +0000
- Revision:
- 2:8eca67634cba
- Parent:
- 1:11b728aa0077
ticker toegevoegd en juiste kinematica (met frame 1 en 2 op verschillende x positie) toegepast
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JorineOosting | 1:11b728aa0077 | 1 | #include "mbed.h" |
JorineOosting | 1:11b728aa0077 | 2 | #include <math.h> |
JorineOosting | 1:11b728aa0077 | 3 | |
JorineOosting | 1:11b728aa0077 | 4 | // Define variables |
JorineOosting | 1:11b728aa0077 | 5 | // Lengtes zijn in meters |
JorineOosting | 1:11b728aa0077 | 6 | // Vaste variabelen: |
Marle | 2:8eca67634cba | 7 | const double L1 = 0.208; // Hoogte van tafel tot joint 1 |
Marle | 2:8eca67634cba | 8 | const double L2 = 0.288; // Hoogte van tafel tot joint 2 |
JorineOosting | 1:11b728aa0077 | 9 | const double L3 = 0.212; // Lengte van de arm |
Marle | 2:8eca67634cba | 10 | const double L4 = 0.089; // Afstand van achterkant base tot joint 1 |
Marle | 2:8eca67634cba | 11 | const double L5 = 0.030; // Afstand van joint 1 naar joint 2 |
JorineOosting | 1:11b728aa0077 | 12 | const double r_trans = 0.035; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation |
JorineOosting | 1:11b728aa0077 | 13 | const double T = 0.002f; // Ticker value |
Marle | 2:8eca67634cba | 14 | Ticker ref_tick; // Ticker aanmaken |
JorineOosting | 1:11b728aa0077 | 15 | |
JorineOosting | 1:11b728aa0077 | 16 | |
JorineOosting | 1:11b728aa0077 | 17 | // Variërende variabelen: |
JorineOosting | 1:11b728aa0077 | 18 | double q1 = 0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is |
JorineOosting | 1:11b728aa0077 | 19 | double q2 = 0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is |
JorineOosting | 1:11b728aa0077 | 20 | double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals |
JorineOosting | 1:11b728aa0077 | 21 | double v_y; // Desired velocity end effecftor in y direction --> Determined by EMG signals |
JorineOosting | 1:11b728aa0077 | 22 | |
JorineOosting | 1:11b728aa0077 | 23 | double Lq1; // Translatieafstand als gevolg van motor rotation joint 1 |
JorineOosting | 1:11b728aa0077 | 24 | double Cq2; // Joint angle of the system (corrected for gear ratio 1:5) |
JorineOosting | 1:11b728aa0077 | 25 | |
JorineOosting | 1:11b728aa0077 | 26 | double q1_dot; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken |
JorineOosting | 1:11b728aa0077 | 27 | double q2_dot; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken |
JorineOosting | 1:11b728aa0077 | 28 | |
JorineOosting | 1:11b728aa0077 | 29 | double q1_ii; // Reference signal for motorangle q1 |
JorineOosting | 1:11b728aa0077 | 30 | double q2_ii; // Reference signal for motorangle q2 |
JorineOosting | 1:11b728aa0077 | 31 | |
JorineOosting | 1:11b728aa0077 | 32 | |
Marle | 2:8eca67634cba | 33 | void inverse_kinematics{ |
JorineOosting | 1:11b728aa0077 | 34 | Lq1 = q1*r_trans; |
JorineOosting | 1:11b728aa0077 | 35 | Cq2 = q2/5.0; |
JorineOosting | 1:11b728aa0077 | 36 | |
Marle | 2:8eca67634cba | 37 | q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2)); |
Marle | 2:8eca67634cba | 38 | q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2)); |
JorineOosting | 1:11b728aa0077 | 39 | |
JorineOosting | 1:11b728aa0077 | 40 | q1_ii = q1 + q1_dot*T; |
JorineOosting | 1:11b728aa0077 | 41 | q2_ii = q2 + q2_dot*T; |
JorineOosting | 1:11b728aa0077 | 42 | |
JorineOosting | 1:11b728aa0077 | 43 | q1 = q1_ii; |
Marle | 2:8eca67634cba | 44 | q2 = q2_ii; |
Marle | 2:8eca67634cba | 45 | } |
Marle | 2:8eca67634cba | 46 | |
Marle | 2:8eca67634cba | 47 | |
Marle | 2:8eca67634cba | 48 | int main() { |
Marle | 2:8eca67634cba | 49 | while (true) { |
Marle | 2:8eca67634cba | 50 | |
Marle | 2:8eca67634cba | 51 | ref_tick.attach(&inverse_kinematics,T); |
JorineOosting | 1:11b728aa0077 | 52 | |
JorineOosting | 1:11b728aa0077 | 53 | } |
JorineOosting | 1:11b728aa0077 | 54 | } |
JorineOosting | 1:11b728aa0077 | 55 |