Inverse kinematica

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 
00003 volatile float q_1;
00004 volatile float q_2;
00005 volatile float r_1;
00006 volatile float r_2;
00007 volatile const float r_3 = 0.035;
00008 
00009 int main()
00010 {
00011     q_1= 1.05;
00012     q_2= 2.5;
00013     r_1= -0.2;
00014     r_2= -0.2;
00015         float u = -r_2*sin(q_1)*cos(q_2)-(r_2)*cos(q_1)*sin(q_2);
00016         float z = 2.0*(r_2*cos(q_1)*cos(q_2))-r_3;
00017         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;
00018         float x = (-2.0)*r_2*sin(q_1)*cos(q_2);
00019         float D =1.0/(u*z-x*y);                         // Determinant
00020         printf("Determinant is %f\n", D);
00021         
00022         float a = D*z;                                  // Inverse jacobian a,b,c,d vormen 2 bij 2 matrix
00023         float b = -D*x;                                 // Inverse jacobian
00024         float c = -D*y;                                 // Inverse jacobian
00025         float d = D*u;                                  // Inverse jacobian
00026         
00027         float vx = 0.01;                                // uit emg data
00028         float vy = 0.0;                                 // uit emg data
00029         float w_1 = vx*a+vy*b;
00030         float w_2 = vx*c+vy*d;
00031         printf("%f\n", w_1);
00032         printf("%f\n", w_2);
00033         
00034  /*       printf("a is%f\n", a);
00035         printf("%f\n", b);
00036         printf("%f\n", c);
00037         printf("%f\n", d);
00038 */                
00039  /*   float Adj_jac [2][2];
00040     Adj_jac[0][0] = 2.0;
00041     Adj_jac[0][1] = 3.0;
00042     Adj_jac[1][0] = 4.0;
00043     Adj_jac[1][1] = 5.0;
00044     printarray("%f", Adj_jac);
00045 */ 
00046 
00047 }