Lennart van Hoorne / Mbed OS Function_IK

Functie Inverse Kinematica

Dependencies:   HIDScope MODSERIAL

main.cpp@1:f3afd35a7188, 2018-10-30 (annotated)

Committer:
LennartvanHoorne
Date:
Tue Oct 30 09:52:50 2018 +0000
Revision:
1:f3afd35a7188
Parent:
0:4fc925102d99
Child:
2:5abed9475e52
Functie Inverse Kinematica

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LennartvanHoorne 0:4fc925102d99 1 #include "mbed.h"
LennartvanHoorne 0:4fc925102d99 2 #include "math.h"
LennartvanHoorne 1:f3afd35a7188 3 #include "HIDScope.h"
LennartvanHoorne 0:4fc925102d99 4
LennartvanHoorne 0:4fc925102d99 5 // Constantes
LennartvanHoorne 0:4fc925102d99 6 const double L1 = 0.35;
LennartvanHoorne 0:4fc925102d99 7 const double L2 = 0.30;
LennartvanHoorne 1:f3afd35a7188 8 const double T = 3;
LennartvanHoorne 0:4fc925102d99 9
LennartvanHoorne 0:4fc925102d99 10 //Variables
LennartvanHoorne 0:4fc925102d99 11 double q1;
LennartvanHoorne 0:4fc925102d99 12 double q2;
LennartvanHoorne 0:4fc925102d99 13 double vx;
LennartvanHoorne 0:4fc925102d99 14 double vy;
LennartvanHoorne 0:4fc925102d99 15
LennartvanHoorne 1:f3afd35a7188 16
LennartvanHoorne 0:4fc925102d99 17 //Hoe werken de variabls?
LennartvanHoorne 0:4fc925102d99 18 // q1 en q2 komen uit de Encoder dus die worden geleverd
LennartvanHoorne 0:4fc925102d99 19 // This is the function for the Inverse Kinematics We start with a inverse Jacobian so we can determine q_dot (rotation speed of the motors)
LennartvanHoorne 0:4fc925102d99 20
LennartvanHoorne 1:f3afd35a7188 21 double Inverse_Kinetmatics(&q1 &q2 &q1_new &q2_new){
LennartvanHoorne 0:4fc925102d99 22
LennartvanHoorne 0:4fc925102d99 23 double invj[2][2] =
LennartvanHoorne 0:4fc925102d99 24 {
LennartvanHoorne 0:4fc925102d99 25 {sin(q1 + q2)/(L1*cos(q1 + q2)*sin(q1) - L1*sin(q1 + q2)*cos(q1)), -cos(q1 + q2)/(L1*cos(q1 + q2)*sin(q1) - L1*sin(q1 + q2)*cos(q1))},
LennartvanHoorne 0:4fc925102d99 26 {-(L2*sin(q1 + q2) + L1*sin(q1))/(L1*L2*cos(q1 + q2)*sin(q1) - L1*L2*sin(q1 + q2)*cos(q1)), (L2*cos(q1 + q2) + L1*cos(q1))/(L1*L2*cos(q1 + q2)*sin(q1) - L1*L2*sin(q1 + q2)*cos(q1))}
LennartvanHoorne 0:4fc925102d99 27 };
LennartvanHoorne 0:4fc925102d99 28
LennartvanHoorne 0:4fc925102d99 29 double V_q1 = invj[1][1]*vx + invj[1][2]*vy;
LennartvanHoorne 0:4fc925102d99 30 double V_q2 = invj[2][1]*vx + invj[2][2]*vy;
LennartvanHoorne 0:4fc925102d99 31
LennartvanHoorne 1:f3afd35a7188 32 // Numerical Integral to make it position controlled
LennartvanHoorne 1:f3afd35a7188 33 double q1_new = q1 + V_q1*T;
LennartvanHoorne 1:f3afd35a7188 34 q1 = q1_new;
LennartvanHoorne 1:f3afd35a7188 35 double q2_new = q2 + V_q2*T;
LennartvanHoorne 1:f3afd35a7188 36 q2 = q2_new;
LennartvanHoorne 1:f3afd35a7188 37 }
LennartvanHoorne 0:4fc925102d99 38
LennartvanHoorne 0:4fc925102d99 39
LennartvanHoorne 0:4fc925102d99 40