Isabella Waayer
/
Inverse_Kinematics2
Test 2 Inverse_Kinematics (Makkelijkere versie)
Diff: TestRKI.cpp
- Revision:
- 2:8eca67634cba
- Parent:
- 1:11b728aa0077
- Child:
- 3:6aa73c8a0489
--- a/TestRKI.cpp Tue Oct 30 18:02:23 2018 +0000 +++ b/TestRKI.cpp Tue Oct 30 18:21:31 2018 +0000 @@ -4,13 +4,14 @@ // 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 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 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 +Ticker ref_tick; // Ticker aanmaken // Variërende variabelen: @@ -29,22 +30,25 @@ double q2_ii; // Reference signal for motorangle q2 -int main() { - while (true) { - - ticker.attach(ref_ticker); - +void inverse_kinematics{ 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_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; + q2 = q2_ii; + } + + +int main() { + while (true) { + + ref_tick.attach(&inverse_kinematics,T); } }