Isabella Waayer
/
Inverse_Kinematics2
Test 2 Inverse_Kinematics (Makkelijkere versie)
TestRKI.cpp
- Committer:
- s1961438
- Date:
- 2019-10-18
- Revision:
- 3:6aa73c8a0489
- Parent:
- 2:8eca67634cba
File content as of revision 3:6aa73c8a0489:
#include "mbed.h" #include <math.h> //om dingen zoals sqrt te kunnen laten werken // Define variables // Lengtes zijn in meters // Vaste variabelen: 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 // 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 void inverse_kinematics { Lq1 = q1*r_trans; Cq2 = q2/5.0; q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2)); q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2)); q1_ii = q1 + q1_dot*T; q2_ii = q2 + q2_dot*T; q1 = q1_ii; q2 = q2_ii; } int main() { while (true) { ref_tick.attach(&inverse_kinematics,T); } }