Diff: main.cpp

Revision:

10:93a76bd81eef

Parent:

9:831891bb0d11

Child:

11:44b1c5b3b378

--- a/main.cpp	Mon Nov 02 11:55:24 2015 +0000
+++ b/main.cpp	Mon Nov 02 12:30:04 2015 +0000
@@ -112,7 +112,7 @@
 double phi_two_curr = 0;
 
 // REFERENCE SETTINGS
-    double input_threshold = 0.5;   // the minimum value the signal must have in order to change the reference.
+    double input_threshold = 0.25;   // the minimum value the signal must have in order to change the reference.
  // Define storage variables for reference values (also start position)
     double c_reference_x = 60, c_reference_y = 0;
 // x-settings   (no y-settings because these are calculated from the current x-position)
@@ -193,23 +193,7 @@
     const double denlow5_2 =-1.95557824031504;
     const double denlow5_3 =0.956543676511203;    
     
-    
- // programmed movements
-// counter
-double mt1 = 0;
-// time
-double T1 = 4, T2 = T1 + 2, T3 = T2 + 6, T4 = T3 + 1, T5 = T4 + 6, T6 = T5 + 4, T7 = T6 + 4, T8 = T7 + 1;
-
-// pak beker
-double x1 = 50, y1 = -37;
-double x2 = 60, y2 = -37;
-double x3 = 60, y3 = 30;
-double x4 = 60, y4 = 30;
-double x5 = 60, y5 = -37;
-double x6 = 50, y6 = -37 ;
-double x7 = 50, y7 = 0;  
-double x8 = 50, y8 = 0;
-     
+   
 ////////////////////////////////////////////////////////////////
 /////////////////// START OF SIDE FUNCTIONS ////////////////////
 //////////////////////////////////////////////////////////////
@@ -352,58 +336,7 @@
     }
 }
 
-// this function allows the xx and yy variables to follow a figure determined by a set of coordinates
-// this function is very simple, can be (possibly) improved by writing a single loop.
-void square_move()
-{
-    if (mt1 > 0 && mt1 < T1*controlfreq)     
-    {
-        xx = x8 + (x1-x8)*(mt1/(T1*controlfreq));
-        yy = y8 + (y1-y8)*(mt1/(T1*controlfreq));
-    }
-    else if  (mt1 >= T1*controlfreq && mt1 < T2*controlfreq) 
-    {
-        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) 
-    {
-        xx = x2 + (x3-x2)*(mt1-T2*controlfreq)/(T3*controlfreq-T2*controlfreq) ;
-        yy = y2 + (y3-y2)*(mt1-T2*controlfreq)/(T3*controlfreq-T2*controlfreq);
-    }
-    else if  (mt1 >= T3*controlfreq && mt1 < T4*controlfreq) 
-    {
-        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 && mt1 < T5*controlfreq) 
-    {
-        xx = x4 + (x5-x4)*(mt1-T4*controlfreq)/(T5*controlfreq-T4*controlfreq) ;
-        yy = y4 + (y5-y4)*(mt1-T4*controlfreq)/(T5*controlfreq-T4*controlfreq) ;
-    }
-    else if  (mt1 >= T5*controlfreq && mt1 < T6*controlfreq)
-    {
-        xx = x5 + (x6-x5)*(mt1-T5*controlfreq)/(T6*controlfreq-T5*controlfreq) ;
-        yy = y5 + (y6-y5)*(mt1-T5*controlfreq)/(T6*controlfreq-T5*controlfreq) ;
-    }
-    else if  (mt1 >= T6*controlfreq && mt1 < T7*controlfreq)
-    {
-        xx = x6 + (x7-x6)*(mt1-T6*controlfreq)/(T7*controlfreq-T6*controlfreq) ;
-        yy = y6 + (y7-y6)*(mt1-T6*controlfreq)/(T7*controlfreq-T6*controlfreq) ;
-    }
-    else if  (mt1 >= T7*controlfreq && mt1 < T8*controlfreq)
-    {
-        xx = x7 + (x8-x7)*(mt1-T7*controlfreq)/(T8*controlfreq-T7*controlfreq) ;
-        yy = y7 + (y8-y7)*(mt1-T7*controlfreq)/(T8*controlfreq-T7*controlfreq) ;
-    }
-    else if (mt1 >= T8*controlfreq)
-    {
-        mt1 = 0;
-        xx = x8;
-        yy = y8;
-    }
-    mt1++;
-}
+
 /////////////////////////////////////////////////////////////////////
 ////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
 ///////////////////////////////////////////////////////////////////
@@ -474,15 +407,6 @@
     // use the signal to change the y-reference
     if(switch_xy == true){yy = reference_f(xy_main_input,c_reference_y,y_min,y_max,Vmax_y);}
     
-    // start the pre-programmed movement
-    // if(program){square_move();}
-    
-    // check the references (otherwise unwanted behavior)
-    //if(yy < y_min){yy = y_min;}
-    //if(yy > y_max){yy = y_max;}
-    
-    // if(xx > x_max){xx = x_max;}
-    
     // x-y to arm-angles math
     double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector
     double alfa = acos((2*pow(L,2)-pow(r,2))/(2*pow(L,2))); // alfa is de hoek tussen upper en lower arm