Diff: main.cpp

Revision:

3:ebd94237935e

Parent:

2:85ab9173d947

Child:

4:9684b6f8b63c

--- a/main.cpp	Wed Oct 28 12:29:23 2015 +0000
+++ b/main.cpp	Wed Oct 28 13:15:43 2015 +0000
@@ -200,16 +200,16 @@
 // time
 double T1 = 4, T2 = 8, T3 = 12, T4 = 16;
 // x,y coordinates
-double x1 = 55, y1 = -15;
-double x2 = 55, y2 = 15;
-double x3 = 65, y3 = 15;
-double x4 = 65, y4 = -15;
+//double x1 = 55, y1 = -15;
+//double x2 = 55, y2 = 15;
+//double x3 = 65, y3 = 15;
+//double x4 = 65, y4 = -15;
 
-// paker beker
-//double x1 = 60, y1 = -37;
-//double x2 = 55, y2 = 0;
-//double x3 = 50, y3 = 0;
-//double x4 = 50, y4 = -37;  
+// pak beker
+double x1 = 50, y1 = -37;
+double x2 = 60, y2 = -37;
+double x3 = 60, y3 = 30;
+double x4 = 60, y4 = -37;  
      
 ////////////////////////////////////////////////////////////////
 /////////////////// START OF SIDE FUNCTIONS ////////////////////
@@ -345,21 +345,21 @@
     if (mt1 > 0 && mt1 < T1*controlfreq)     // horizontal movement from (65,-20) -> (55,-20)
     {
         xx =  x4 + (x1-x4)*(mt1/(T1*controlfreq));
-        yy = y4;
+        yy = y4 + (y1-y4)*(mt1/(T1*controlfreq));
     }
     else if  (mt1 >= T1*controlfreq && mt1 < T2*controlfreq) // vertical movement (55,-20) -> (55,20)
     {
-        xx = x1;
+        xx = x1 + (x2-x1)*(mt1-T1*controlfreq)/(T2*controlfreq-T1*controlfreq) ;
         yy = y1 + (y2-y1)*(mt1-T1*controlfreq)/(T2*controlfreq-T1*controlfreq) ; 
     }
     else if  (mt1 >= T2*controlfreq && mt1 < T3*controlfreq) // horizontal movement (55,20) -> (65,20)
     {
         xx = x2 + (x3-x2)*(mt1-T2*controlfreq)/(T3*controlfreq-T2*controlfreq) ;
-        yy = y2;
+        yy = y2 + (y3-y2)*(mt1-T2*controlfreq)/(T3*controlfreq-T2*controlfreq);
     }
     else if  (mt1 >= T3*controlfreq && mt1 < T4*controlfreq) // vertical movement (65,20) -> (65,-20)
     {
-        xx = x3;
+        xx = x3 + (x4-x3)*(mt1-T3*controlfreq)/(T4*controlfreq-T3*controlfreq) ;
         yy = y3 + (y4-y3)*(mt1-T3*controlfreq)/(T4*controlfreq-T3*controlfreq) ;
     }
     else if (mt1 >= T4*controlfreq)
@@ -399,11 +399,11 @@
     // update global variables
     output2 = y5t;
     output2_amp = y5t*emg_gain2; 
-    scope.set(0,output1_amp);
-    scope.set(1,output2_amp);
-    scope.set(2,u1);
-    scope.set(3,u1t);
-    scope.send();
+    //scope.set(0,output1_amp);
+    //scope.set(1,output2_amp);
+    //scope.set(2,u1);
+    //scope.set(3,u1t);
+    //scope.send();
  }
  
  
@@ -462,6 +462,10 @@
     if(yy < y_min){yy = y_min;}
     if(yy > y_max){yy = y_max;}
     
+    scope.set(0,xx);
+    scope.set(1,yy);
+    scope.send();
+    
     // let the arm make a circle (testing)
     // xx = 60 + 5*cos(t);
     // yy = 5*sin(t);