Isabella Waayer
/
Inverse_Kinematics2
Test 2 Inverse_Kinematics (Makkelijkere versie)
Diff: TestRKI.cpp
- Revision:
- 1:11b728aa0077
- Child:
- 2:8eca67634cba
diff -r 00721d3e1cf7 -r 11b728aa0077 TestRKI.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TestRKI.cpp Tue Oct 30 18:02:23 2018 +0000 @@ -0,0 +1,51 @@ +#include "mbed.h" +#include <math.h> + +// Define variables +// Lengtes zijn in meters +// Vaste variabelen: +const double L0 = 0.088; // Afstand achterkant robot tot joint 2 +const double L1 = 0.288; // Hoogte van tafel tot joint 2 +const double L2 = 0.208; // Hoogte van tafel tot joint 1 +const double L3 = 0.212; // Lengte van de arm +const double r_trans = 0.035; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation +const double T = 0.002f; // Ticker value +Ticker ref_ticker; // Ticker aanmaken + + +// Variërende variabelen: +double q1 = 0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is +double q2 = 0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is +double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals +double v_y; // Desired velocity end effecftor in y direction --> Determined by EMG signals + +double Lq1; // Translatieafstand als gevolg van motor rotation joint 1 +double Cq2; // Joint angle of the system (corrected for gear ratio 1:5) + +double q1_dot; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken +double q2_dot; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken + +double q1_ii; // Reference signal for motorangle q1 +double q2_ii; // Reference signal for motorangle q2 + + +int main() { + while (true) { + + ticker.attach(ref_ticker); + + Lq1 = q1*r_trans; + Cq2 = q2/5.0; + + q1_dot = (v_x + (v_y*(L2 + L3*sin(q2/5.0)))/(L0 + q1*r_trans + L3*cos(q2/5.0)))/r_trans; + q2_dot = (5.0*v_y)/(L0 + q1*r_trans + L3*cos(q2/5.0)); + + q1_ii = q1 + q1_dot*T; + q2_ii = q2 + q2_dot*T; + + q1 = q1_ii; + q2 = q2_ii; + + } +} +