Isabella Waayer / Mbed OS Inverse_Kinematics2

Test 2 Inverse_Kinematics (Makkelijkere versie)

Dependencies:   Matrix

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?

UserRevisionLine numberNew 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