Diff: main.cpp

Revision:

2:4720140f7953

Parent:

1:8eb1c39a91be

--- a/main.cpp	Fri Oct 23 08:55:02 2015 +0000
+++ b/main.cpp	Fri Oct 23 09:35:03 2015 +0000
@@ -17,12 +17,12 @@
 
 Ticker      signal_emg;         //Ticker for signal simulation
 
-double      emg_1 = 0.1;
-double      emg_2 = 0.05;
+double      emg_1 = 10;
+double      emg_2 = 5;
 double      l1 = 0.25;
 double      l2 = 0.25;
-double      x = 30;
-double      y = 20;
+double      x = 0.3;
+double      y = 0.2;
 double      rpc;
 double      theta1;
 double      theta2;
@@ -39,8 +39,8 @@
 
 void take_sample()
 {
-    emg_1 = emg_1 + 0.1;
-    emg_2 = emg_2 + 0.05;
+    emg_1 = emg_1 + 20;
+    emg_2 = emg_2 + 10;
 }
 
 //Current position - Encoder counts -> current angle -> Forward Kinematics
@@ -50,7 +50,7 @@
     while(true) {
         if (button == 0) {
             pc.printf("knop1\n");
-            while(emg_1<2) {
+            while(true) {
                 signal_emg.attach(&take_sample,0.5);
                 pc.printf("emg1 = %f\n",emg_1);
 
@@ -59,13 +59,17 @@
                 theta2 = emg_2 * rpc;
                 current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2);
                 current_y = l1 * sin(theta1) + l2 * sin(theta1 + theta2);
+                
+                double theta1_degree = theta1 * (180/PI);
+                double theta2_degree = theta2 * (180/PI);
 
-                pc.printf("Theta1 = %f, theta2 = %f \r\n",theta1,theta2);
+                pc.printf("Theta1 = %f, theta2 = %f \r\n",theta1_degree,theta2_degree);
 
                 //calculate error (refpos-currentpos) currentpos = forward kinematics
                 x_error = x - current_x;
                 y_error = y - current_y;
-
+                
+                pc.printf("Error x is %f, error y is %f \r\n",x_error,y_error);
 
                 //inverse kinematics (refpos to refangle)
 
@@ -79,6 +83,11 @@
                 double k2 = l2*sintheta2;
 
                 dtheta1 = atan2(y, x) - atan2(k2, k1);
+                
+                double dtheta1_degree = dtheta1 * (180/PI);
+                double dtheta2_degree = dtheta2 * (180/PI);
+                
+                pc.printf("Wanted theta1 is %f, wanted theta2 is %f \r\n",dtheta1_degree,dtheta2_degree); 
 
                 /* alternative:
                 costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sintheta2 ) / ( pow(x,2) + pow(y,2) );
@@ -93,6 +102,11 @@
 
                 pc.printf("Motor 1 error is %f, motor 2 error is %f \r\n",m1_error,m2_error);
                 wait(0.5);
+                
+                if(x_error>0.002&&x_error<-0.002&&y_error>0.002&&y_error<-0.002)
+                {
+                    break;
+                }
             }
         }
     }