Lennart van Hoorne / Mbed OS Function_IK

Functie Inverse Kinematica

Dependencies:   HIDScope MODSERIAL

main.cpp@5:205f1de452de, 2018-10-30 (annotated)

Committer:
LennartvanHoorne
Date:
Tue Oct 30 14:02:01 2018 +0000
Revision:
5:205f1de452de
Parent:
4:a97c2e8a80d8
Child:
6:0256de2854d6
Proberen met whileloop

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