Jurriën Bos
/
Inverse
Inverse kinematica
Diff: main.cpp
- Revision:
- 0:a204dfa75158
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Oct 26 15:19:17 2018 +0000 @@ -0,0 +1,47 @@ +#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); +*/ + +}