Inverse kinematica
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Wed Jul 13 2022 01:58:22 by
1.7.2