![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Test 2 Inverse_Kinematics (Makkelijkere versie)
Diff: TestRKI.cpp
- Revision:
- 3:6aa73c8a0489
- Parent:
- 2:8eca67634cba
--- a/TestRKI.cpp Tue Oct 30 18:21:31 2018 +0000 +++ b/TestRKI.cpp Fri Oct 18 12:37:38 2019 +0000 @@ -1,14 +1,14 @@ #include "mbed.h" -#include <math.h> +#include <math.h> //om dingen zoals sqrt te kunnen laten werken // Define variables // Lengtes zijn in meters // Vaste variabelen: -const double L1 = 0.208; // Hoogte van tafel tot joint 1 -const double L2 = 0.288; // Hoogte van tafel tot joint 2 -const double L3 = 0.212; // Lengte van de arm -const double L4 = 0.089; // Afstand van achterkant base tot joint 1 -const double L5 = 0.030; // Afstand van joint 1 naar joint 2 +const double L1 = 0.05; // Hoogte van base plaat tot joint 1 = afhankelijk van de patient +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 +const double L3 = 0.30; // Lengte van de arm 1 +//const double L4 = 0.089; // Afstand van achterkant base tot joint 1 +const double L5 = 0.30; // Afstand van joint 1 naar joint 2 = arm 2 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_tick; // Ticker aanmaken @@ -21,7 +21,7 @@ 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 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 @@ -30,7 +30,8 @@ double q2_ii; // Reference signal for motorangle q2 -void inverse_kinematics{ +void inverse_kinematics +{ Lq1 = q1*r_trans; Cq2 = q2/5.0; @@ -42,14 +43,14 @@ q1 = q1_ii; q2 = q2_ii; - } +} int main() { - while (true) { + while (true) +{ ref_tick.attach(&inverse_kinematics,T); - - } + } }