Lennart van Hoorne / Mbed OS Function_IK

Functie Inverse Kinematica

Dependencies:   HIDScope MODSERIAL

main.cpp@3:2a437a6ef149, 2018-10-30 (annotated)

Committer:
LennartvanHoorne
Date:
Tue Oct 30 10:56:28 2018 +0000
Revision:
3:2a437a6ef149
Parent:
2:5abed9475e52
Child:
4:a97c2e8a80d8
Probleem met de functie Inverse_Kinematics

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 3:2a437a6ef149 3 #include "HIDScope.h"
LennartvanHoorne 3:2a437a6ef149 4
LennartvanHoorne 3:2a437a6ef149 5 // Define the HIDScope and Ticker object
LennartvanHoorne 3:2a437a6ef149 6 HIDScope scope(2);
LennartvanHoorne 3:2a437a6ef149 7 Ticker scopeTimer;
LennartvanHoorne 3:2a437a6ef149 8 Ticker IKTicker;
LennartvanHoorne 3:2a437a6ef149 9
LennartvanHoorne 0:4fc925102d99 10
LennartvanHoorne 0:4fc925102d99 11 // Constantes
LennartvanHoorne 0:4fc925102d99 12 const double L1 = 0.35;
LennartvanHoorne 0:4fc925102d99 13 const double L2 = 0.30;
LennartvanHoorne 1:f3afd35a7188 14 const double T = 3;
LennartvanHoorne 0:4fc925102d99 15
LennartvanHoorne 0:4fc925102d99 16 //Variables
LennartvanHoorne 0:4fc925102d99 17 double q1;
LennartvanHoorne 0:4fc925102d99 18 double q2;
LennartvanHoorne 0:4fc925102d99 19 double vx;
LennartvanHoorne 0:4fc925102d99 20 double vy;
LennartvanHoorne 0:4fc925102d99 21
LennartvanHoorne 1:f3afd35a7188 22
LennartvanHoorne 0:4fc925102d99 23 //Hoe werken de variabls?
LennartvanHoorne 0:4fc925102d99 24 // q1 en q2 komen uit de Encoder dus die worden geleverd
LennartvanHoorne 0:4fc925102d99 25 // 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 26
LennartvanHoorne 3:2a437a6ef149 27 void Inverse_Kinetmatics(&q1 &q2 &q1_new &q2_new) {
LennartvanHoorne 0:4fc925102d99 28
LennartvanHoorne 0:4fc925102d99 29 double invj[2][2] =
LennartvanHoorne 0:4fc925102d99 30 {
LennartvanHoorne 0:4fc925102d99 31 {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 32 {-(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 33 };
LennartvanHoorne 0:4fc925102d99 34
LennartvanHoorne 0:4fc925102d99 35 double V_q1 = invj[1][1]*vx + invj[1][2]*vy;
LennartvanHoorne 0:4fc925102d99 36 double V_q2 = invj[2][1]*vx + invj[2][2]*vy;
LennartvanHoorne 0:4fc925102d99 37
LennartvanHoorne 1:f3afd35a7188 38 // Numerical Integral to make it position controlled
LennartvanHoorne 1:f3afd35a7188 39 double q1_new = q1 + V_q1*T;
LennartvanHoorne 1:f3afd35a7188 40 q1 = q1_new;
LennartvanHoorne 1:f3afd35a7188 41 double q2_new = q2 + V_q2*T;
LennartvanHoorne 1:f3afd35a7188 42 q2 = q2_new;
LennartvanHoorne 1:f3afd35a7188 43 }
LennartvanHoorne 3:2a437a6ef149 44
LennartvanHoorne 3:2a437a6ef149 45 // Write a function where the data gets send to HIDScope
LennartvanHoorne 3:2a437a6ef149 46 void scopeSend()
LennartvanHoorne 3:2a437a6ef149 47 {
LennartvanHoorne 3:2a437a6ef149 48 scope.set(0,q1());
LennartvanHoorne 3:2a437a6ef149 49 scope.set(1,q2());
LennartvanHoorne 3:2a437a6ef149 50 scope.send();
LennartvanHoorne 3:2a437a6ef149 51 }
LennartvanHoorne 0:4fc925102d99 52
LennartvanHoorne 3:2a437a6ef149 53
LennartvanHoorne 3:2a437a6ef149 54 int main()
LennartvanHoorne 3:2a437a6ef149 55 {
LennartvanHoorne 3:2a437a6ef149 56 // Attach the data read and send function at 100 Hz
LennartvanHoorne 3:2a437a6ef149 57 IKTicker.attach_us(&Inverse_Kinematics, 0.1);
LennartvanHoorne 3:2a437a6ef149 58 // Attach the data read and send function at 100 Hz
LennartvanHoorne 3:2a437a6ef149 59 scopeTimer.attach_us(&scopeSend, 2e3);
LennartvanHoorne 3:2a437a6ef149 60
LennartvanHoorne 3:2a437a6ef149 61 while(1) { }
LennartvanHoorne 3:2a437a6ef149 62 }