![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Inverse kinematica
main.cpp
- Committer:
- JurrienBos
- Date:
- 2018-10-26
- Revision:
- 0:a204dfa75158
File content as of revision 0:a204dfa75158:
#include "mbed.h" volatile float q_1; volatile float q_2; volatile float r_1; volatile float r_2; volatile const float r_3 = 0.035; int main() { q_1= 1.05; q_2= 2.5; r_1= -0.2; r_2= -0.2; float u = -r_2*sin(q_1)*cos(q_2)-(r_2)*cos(q_1)*sin(q_2); float z = 2.0*(r_2*cos(q_1)*cos(q_2))-r_3; 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; float x = (-2.0)*r_2*sin(q_1)*cos(q_2); float D =1.0/(u*z-x*y); // Determinant printf("Determinant is %f\n", D); float a = D*z; // Inverse jacobian a,b,c,d vormen 2 bij 2 matrix float b = -D*x; // Inverse jacobian float c = -D*y; // Inverse jacobian float d = D*u; // Inverse jacobian float vx = 0.01; // uit emg data float vy = 0.0; // uit emg data float w_1 = vx*a+vy*b; float w_2 = vx*c+vy*d; printf("%f\n", w_1); printf("%f\n", w_2); /* printf("a is%f\n", a); printf("%f\n", b); printf("%f\n", c); printf("%f\n", d); */ /* float Adj_jac [2][2]; Adj_jac[0][0] = 2.0; Adj_jac[0][1] = 3.0; Adj_jac[1][0] = 4.0; Adj_jac[1][1] = 5.0; printarray("%f", Adj_jac); */ }