Diff: main.cpp

Revision:

2:85ab9173d947

Parent:

1:4c9994ac229c

Child:

3:ebd94237935e

--- a/main.cpp	Tue Oct 27 14:08:23 2015 +0000
+++ b/main.cpp	Wed Oct 28 12:29:23 2015 +0000
@@ -191,7 +191,25 @@
     const double numlow5_2 =0.000482718098083923;
     const double numlow5_3 =0.000241359049041961;
     const double denlow5_2 =-1.95557824031504;
-    const double denlow5_3 =0.956543676511203;      
+    const double denlow5_3 =0.956543676511203;    
+    
+    
+ // programmed movements
+// counter
+double mt1 = 0;
+// 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;
+
+// paker beker
+//double x1 = 60, y1 = -37;
+//double x2 = 55, y2 = 0;
+//double x3 = 50, y3 = 0;
+//double x4 = 50, y4 = -37;  
      
 ////////////////////////////////////////////////////////////////
 /////////////////// START OF SIDE FUNCTIONS ////////////////////
@@ -321,6 +339,37 @@
 {
     pc.printf("xx = %f, yy = %f, phi1 = %f, phi2 = %f \n",xx,yy,phi_one,phi_two);
 }
+
+void square_move()
+{
+    if (mt1 > 0 && mt1 < T1*controlfreq)     // horizontal movement from (65,-20) -> (55,-20)
+    {
+        xx =  x4 + (x1-x4)*(mt1/(T1*controlfreq));
+        yy = y4;
+    }
+    else if  (mt1 >= T1*controlfreq && mt1 < T2*controlfreq) // vertical movement (55,-20) -> (55,20)
+    {
+        xx = x1;
+        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;
+    }
+    else if  (mt1 >= T3*controlfreq && mt1 < T4*controlfreq) // vertical movement (65,20) -> (65,-20)
+    {
+        xx = x3;
+        yy = y3 + (y4-y3)*(mt1-T3*controlfreq)/(T4*controlfreq-T3*controlfreq) ;
+    }
+    else if (mt1 >= T4*controlfreq)
+    {
+        mt1 = 0;
+        xx = x4;
+        yy = y4;
+    }
+    mt1++;
+}
 /////////////////////////////////////////////////////////////////////
 ////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
 ///////////////////////////////////////////////////////////////////
@@ -394,21 +443,21 @@
     if(xy_main_input>1) {xy_main_input = 1;}
     if(xy_main_input<-1) {xy_main_input = -1;}
  
+ 
+        // calculate the y limits belonging to that particular x coordinate and update global variables
+    y_min = - sqrt(5184 - pow(xx,2));
+    if(y_min<y_min_max){y_min = y_min_max;} // make sure the arm cannot hit the table (may later be removed)
+    y_max = sqrt(5184 - pow(xx,2));
+    
     if(switch_xy == false)          // use the signal to change the x-reference
     {
-        xx = reference_f(xy_main_input,c_reference_x,x_min,x_max,Vmax_x);        // change the global x-reference
-        // calculate the y limits belonging to that particular x coordinate and update global variables
-        y_min = - sqrt(5184 - pow(xx,2));
-        if(y_min<y_min_max){y_min = y_min_max;} // make sure the arm cannot hit the table (may later be removed)
-        y_max = sqrt(5184 - pow(xx,2));
-       
+        xx = reference_f(xy_main_input,c_reference_x,x_min,x_max,Vmax_x);        // change the global x-reference       
     }
     if(switch_xy == true)           // use the signal to change the y-reference
     {
-        yy = reference_f(xy_main_input,c_reference_y,y_min,y_max,Vmax_y);        // change the y-reference
-        
+        yy = reference_f(xy_main_input,c_reference_y,y_min,y_max,Vmax_y);        // change the y-reference 
     }
-    
+    square_move();
     // check the y-reference (otherwise if x is controlled after y has been controlled, the circle is not followed).
     if(yy < y_min){yy = y_min;}
     if(yy > y_max){yy = y_max;}