Isabella Waayer
/
Inverse_Kinematics2
Test 2 Inverse_Kinematics (Makkelijkere versie)
TestRKI.cpp@1:11b728aa0077, 2018-10-30 (annotated)
- Committer:
- JorineOosting
- Date:
- Tue Oct 30 18:02:23 2018 +0000
- Revision:
- 1:11b728aa0077
- Child:
- 2:8eca67634cba
Project RKI test
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: |
JorineOosting | 1:11b728aa0077 | 7 | const double L0 = 0.088; // Afstand achterkant robot tot joint 2 |
JorineOosting | 1:11b728aa0077 | 8 | const double L1 = 0.288; // Hoogte van tafel tot joint 2 |
JorineOosting | 1:11b728aa0077 | 9 | const double L2 = 0.208; // Hoogte van tafel tot joint 1 |
JorineOosting | 1:11b728aa0077 | 10 | const double L3 = 0.212; // Lengte van de arm |
JorineOosting | 1:11b728aa0077 | 11 | const double r_trans = 0.035; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation |
JorineOosting | 1:11b728aa0077 | 12 | const double T = 0.002f; // Ticker value |
JorineOosting | 1:11b728aa0077 | 13 | Ticker ref_ticker; // Ticker aanmaken |
JorineOosting | 1:11b728aa0077 | 14 | |
JorineOosting | 1:11b728aa0077 | 15 | |
JorineOosting | 1:11b728aa0077 | 16 | // Variërende variabelen: |
JorineOosting | 1:11b728aa0077 | 17 | double q1 = 0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is |
JorineOosting | 1:11b728aa0077 | 18 | double q2 = 0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is |
JorineOosting | 1:11b728aa0077 | 19 | double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals |
JorineOosting | 1:11b728aa0077 | 20 | double v_y; // Desired velocity end effecftor in y direction --> Determined by EMG signals |
JorineOosting | 1:11b728aa0077 | 21 | |
JorineOosting | 1:11b728aa0077 | 22 | double Lq1; // Translatieafstand als gevolg van motor rotation joint 1 |
JorineOosting | 1:11b728aa0077 | 23 | double Cq2; // Joint angle of the system (corrected for gear ratio 1:5) |
JorineOosting | 1:11b728aa0077 | 24 | |
JorineOosting | 1:11b728aa0077 | 25 | double q1_dot; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken |
JorineOosting | 1:11b728aa0077 | 26 | double q2_dot; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken |
JorineOosting | 1:11b728aa0077 | 27 | |
JorineOosting | 1:11b728aa0077 | 28 | double q1_ii; // Reference signal for motorangle q1 |
JorineOosting | 1:11b728aa0077 | 29 | double q2_ii; // Reference signal for motorangle q2 |
JorineOosting | 1:11b728aa0077 | 30 | |
JorineOosting | 1:11b728aa0077 | 31 | |
JorineOosting | 1:11b728aa0077 | 32 | int main() { |
JorineOosting | 1:11b728aa0077 | 33 | while (true) { |
JorineOosting | 1:11b728aa0077 | 34 | |
JorineOosting | 1:11b728aa0077 | 35 | ticker.attach(ref_ticker); |
JorineOosting | 1:11b728aa0077 | 36 | |
JorineOosting | 1:11b728aa0077 | 37 | Lq1 = q1*r_trans; |
JorineOosting | 1:11b728aa0077 | 38 | Cq2 = q2/5.0; |
JorineOosting | 1:11b728aa0077 | 39 | |
JorineOosting | 1:11b728aa0077 | 40 | q1_dot = (v_x + (v_y*(L2 + L3*sin(q2/5.0)))/(L0 + q1*r_trans + L3*cos(q2/5.0)))/r_trans; |
JorineOosting | 1:11b728aa0077 | 41 | q2_dot = (5.0*v_y)/(L0 + q1*r_trans + L3*cos(q2/5.0)); |
JorineOosting | 1:11b728aa0077 | 42 | |
JorineOosting | 1:11b728aa0077 | 43 | q1_ii = q1 + q1_dot*T; |
JorineOosting | 1:11b728aa0077 | 44 | q2_ii = q2 + q2_dot*T; |
JorineOosting | 1:11b728aa0077 | 45 | |
JorineOosting | 1:11b728aa0077 | 46 | q1 = q1_ii; |
JorineOosting | 1:11b728aa0077 | 47 | q2 = q2_ii; |
JorineOosting | 1:11b728aa0077 | 48 | |
JorineOosting | 1:11b728aa0077 | 49 | } |
JorineOosting | 1:11b728aa0077 | 50 | } |
JorineOosting | 1:11b728aa0077 | 51 |