Lennart van Hoorne / Mbed OS Function_IK

Functie Inverse Kinematica

Dependencies:   HIDScope MODSERIAL

main.cpp@4:a97c2e8a80d8, 2018-10-30 (annotated)

Committer:
LennartvanHoorne
Date:
Tue Oct 30 12:18:08 2018 +0000
Revision:
4:a97c2e8a80d8
Parent:
3:2a437a6ef149
Child:
5:205f1de452de
Zonder de tikker van de IK, hopelijk kunnen we hem op deze manier downloaden

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