Jurriën Bos / Mbed 2 deprecated Inverse

Inverse kinematica

Dependencies:   mbed

main.cpp@0:a204dfa75158, 2018-10-26 (annotated)

Committer:
JurrienBos
Date:
Fri Oct 26 15:19:17 2018 +0000
Revision:
0:a204dfa75158
Inverse kinematica

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JurrienBos 0:a204dfa75158 1 #include "mbed.h"
JurrienBos 0:a204dfa75158 2
JurrienBos 0:a204dfa75158 3 volatile float q_1;
JurrienBos 0:a204dfa75158 4 volatile float q_2;
JurrienBos 0:a204dfa75158 5 volatile float r_1;
JurrienBos 0:a204dfa75158 6 volatile float r_2;
JurrienBos 0:a204dfa75158 7 volatile const float r_3 = 0.035;
JurrienBos 0:a204dfa75158 8
JurrienBos 0:a204dfa75158 9 int main()
JurrienBos 0:a204dfa75158 10 {
JurrienBos 0:a204dfa75158 11 q_1= 1.05;
JurrienBos 0:a204dfa75158 12 q_2= 2.5;
JurrienBos 0:a204dfa75158 13 r_1= -0.2;
JurrienBos 0:a204dfa75158 14 r_2= -0.2;
JurrienBos 0:a204dfa75158 15 float u = -r_2*sin(q_1)*cos(q_2)-(r_2)*cos(q_1)*sin(q_2);
JurrienBos 0:a204dfa75158 16 float z = 2.0*(r_2*cos(q_1)*cos(q_2))-r_3;
JurrienBos 0:a204dfa75158 17 float y = r_2*cos(q_1)*cos(q_2)-r_2*sin(q_1)*sin(q_2)+2.0*(r_1*cos(q_1))-r_3;
JurrienBos 0:a204dfa75158 18 float x = (-2.0)*r_2*sin(q_1)*cos(q_2);
JurrienBos 0:a204dfa75158 19 float D =1.0/(u*z-x*y); // Determinant
JurrienBos 0:a204dfa75158 20 printf("Determinant is %f\n", D);
JurrienBos 0:a204dfa75158 21
JurrienBos 0:a204dfa75158 22 float a = D*z; // Inverse jacobian a,b,c,d vormen 2 bij 2 matrix
JurrienBos 0:a204dfa75158 23 float b = -D*x; // Inverse jacobian
JurrienBos 0:a204dfa75158 24 float c = -D*y; // Inverse jacobian
JurrienBos 0:a204dfa75158 25 float d = D*u; // Inverse jacobian
JurrienBos 0:a204dfa75158 26
JurrienBos 0:a204dfa75158 27 float vx = 0.01; // uit emg data
JurrienBos 0:a204dfa75158 28 float vy = 0.0; // uit emg data
JurrienBos 0:a204dfa75158 29 float w_1 = vx*a+vy*b;
JurrienBos 0:a204dfa75158 30 float w_2 = vx*c+vy*d;
JurrienBos 0:a204dfa75158 31 printf("%f\n", w_1);
JurrienBos 0:a204dfa75158 32 printf("%f\n", w_2);
JurrienBos 0:a204dfa75158 33
JurrienBos 0:a204dfa75158 34 /* printf("a is%f\n", a);
JurrienBos 0:a204dfa75158 35 printf("%f\n", b);
JurrienBos 0:a204dfa75158 36 printf("%f\n", c);
JurrienBos 0:a204dfa75158 37 printf("%f\n", d);
JurrienBos 0:a204dfa75158 38 */
JurrienBos 0:a204dfa75158 39 /* float Adj_jac [2][2];
JurrienBos 0:a204dfa75158 40 Adj_jac[0][0] = 2.0;
JurrienBos 0:a204dfa75158 41 Adj_jac[0][1] = 3.0;
JurrienBos 0:a204dfa75158 42 Adj_jac[1][0] = 4.0;
JurrienBos 0:a204dfa75158 43 Adj_jac[1][1] = 5.0;
JurrienBos 0:a204dfa75158 44 printarray("%f", Adj_jac);
JurrienBos 0:a204dfa75158 45 */
JurrienBos 0:a204dfa75158 46
JurrienBos 0:a204dfa75158 47 }