Isabella Waayer / Mbed OS Inverse_Kinematics2

Test 2 Inverse_Kinematics (Makkelijkere versie)

Dependencies:   Matrix

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
Child:
3:6aa73c8a0489
ticker toegevoegd en juiste kinematica (met frame 1 en 2 op verschillende x positie) toegepast

Who changed what in which revision?

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