Lennart van Hoorne / Mbed OS Function_IK

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope MODSERIAL

main.cpp@2:5abed9475e52, 2018-10-30 (annotated)

Committer:
LennartvanHoorne
Date:
Tue Oct 30 09:58:20 2018 +0000
Revision:
2:5abed9475e52
Parent:
1:f3afd35a7188
Child:
3:2a437a6ef149
Functie before implementing HIDScope

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 0:4fc925102d99 3
LennartvanHoorne 0:4fc925102d99 4 // Constantes
LennartvanHoorne 0:4fc925102d99 5 const double L1 = 0.35;
LennartvanHoorne 0:4fc925102d99 6 const double L2 = 0.30;
LennartvanHoorne 1:f3afd35a7188 7 const double T = 3;
LennartvanHoorne 0:4fc925102d99 8
LennartvanHoorne 0:4fc925102d99 9 //Variables
LennartvanHoorne 0:4fc925102d99 10 double q1;
LennartvanHoorne 0:4fc925102d99 11 double q2;
LennartvanHoorne 0:4fc925102d99 12 double vx;
LennartvanHoorne 0:4fc925102d99 13 double vy;
LennartvanHoorne 0:4fc925102d99 14
LennartvanHoorne 1:f3afd35a7188 15
LennartvanHoorne 0:4fc925102d99 16 //Hoe werken de variabls?
LennartvanHoorne 0:4fc925102d99 17 // q1 en q2 komen uit de Encoder dus die worden geleverd
LennartvanHoorne 0:4fc925102d99 18 // 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 19
LennartvanHoorne 1:f3afd35a7188 20 double Inverse_Kinetmatics(&q1 &q2 &q1_new &q2_new){
LennartvanHoorne 0:4fc925102d99 21
LennartvanHoorne 0:4fc925102d99 22 double invj[2][2] =
LennartvanHoorne 0:4fc925102d99 23 {
LennartvanHoorne 0:4fc925102d99 24 {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 25 {-(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 26 };
LennartvanHoorne 0:4fc925102d99 27
LennartvanHoorne 0:4fc925102d99 28 double V_q1 = invj[1][1]*vx + invj[1][2]*vy;
LennartvanHoorne 0:4fc925102d99 29 double V_q2 = invj[2][1]*vx + invj[2][2]*vy;
LennartvanHoorne 0:4fc925102d99 30
LennartvanHoorne 1:f3afd35a7188 31 // Numerical Integral to make it position controlled
LennartvanHoorne 1:f3afd35a7188 32 double q1_new = q1 + V_q1*T;
LennartvanHoorne 1:f3afd35a7188 33 q1 = q1_new;
LennartvanHoorne 1:f3afd35a7188 34 double q2_new = q2 + V_q2*T;
LennartvanHoorne 1:f3afd35a7188 35 q2 = q2_new;
LennartvanHoorne 1:f3afd35a7188 36 }
LennartvanHoorne 0:4fc925102d99 37
LennartvanHoorne 0:4fc925102d99 38
LennartvanHoorne 0:4fc925102d99 39