Diff: main.cpp

Revision:

0:a204dfa75158

diff -r 000000000000 -r a204dfa75158 main.cpp
--- /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);
+*/ 
+
+}