Isabella Waayer
/
Inverse_Kinematics2
Test 2 Inverse_Kinematics (Makkelijkere versie)
TestRKI.cpp@3:6aa73c8a0489, 2019-10-18 (annotated)
- Committer:
- s1961438
- Date:
- Fri Oct 18 12:37:38 2019 +0000
- Revision:
- 3:6aa73c8a0489
- Parent:
- 2:8eca67634cba
Test 2 Inverse_Kinematics;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JorineOosting | 1:11b728aa0077 | 1 | #include "mbed.h" |
s1961438 | 3:6aa73c8a0489 | 2 | #include <math.h> //om dingen zoals sqrt te kunnen laten werken |
JorineOosting | 1:11b728aa0077 | 3 | |
JorineOosting | 1:11b728aa0077 | 4 | // Define variables |
JorineOosting | 1:11b728aa0077 | 5 | // Lengtes zijn in meters |
JorineOosting | 1:11b728aa0077 | 6 | // Vaste variabelen: |
s1961438 | 3:6aa73c8a0489 | 7 | const double L1 = 0.05; // Hoogte van base plaat tot joint 1 = afhankelijk van de patient |
s1961438 | 3:6aa73c8a0489 | 8 | const double L2 = 0.05; // Hoogte van base plaat tot joint 2 = dus als hij in de startpositie staat is dit dezelfde hoogte als L2 |
s1961438 | 3:6aa73c8a0489 | 9 | const double L3 = 0.30; // Lengte van de arm 1 |
s1961438 | 3:6aa73c8a0489 | 10 | //const double L4 = 0.089; // Afstand van achterkant base tot joint 1 |
s1961438 | 3:6aa73c8a0489 | 11 | const double L5 = 0.30; // Afstand van joint 1 naar joint 2 = arm 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 |
s1961438 | 3:6aa73c8a0489 | 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 | |
s1961438 | 3:6aa73c8a0489 | 33 | void inverse_kinematics |
s1961438 | 3:6aa73c8a0489 | 34 | { |
JorineOosting | 1:11b728aa0077 | 35 | Lq1 = q1*r_trans; |
JorineOosting | 1:11b728aa0077 | 36 | Cq2 = q2/5.0; |
JorineOosting | 1:11b728aa0077 | 37 | |
Marle | 2:8eca67634cba | 38 | q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2)); |
Marle | 2:8eca67634cba | 39 | q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2)); |
JorineOosting | 1:11b728aa0077 | 40 | |
JorineOosting | 1:11b728aa0077 | 41 | q1_ii = q1 + q1_dot*T; |
JorineOosting | 1:11b728aa0077 | 42 | q2_ii = q2 + q2_dot*T; |
JorineOosting | 1:11b728aa0077 | 43 | |
JorineOosting | 1:11b728aa0077 | 44 | q1 = q1_ii; |
Marle | 2:8eca67634cba | 45 | q2 = q2_ii; |
s1961438 | 3:6aa73c8a0489 | 46 | } |
Marle | 2:8eca67634cba | 47 | |
Marle | 2:8eca67634cba | 48 | |
Marle | 2:8eca67634cba | 49 | int main() { |
s1961438 | 3:6aa73c8a0489 | 50 | while (true) |
s1961438 | 3:6aa73c8a0489 | 51 | { |
Marle | 2:8eca67634cba | 52 | |
Marle | 2:8eca67634cba | 53 | ref_tick.attach(&inverse_kinematics,T); |
s1961438 | 3:6aa73c8a0489 | 54 | } |
JorineOosting | 1:11b728aa0077 | 55 | } |
JorineOosting | 1:11b728aa0077 | 56 |