Diff: main.cpp

Revision:

8:f2f45be5a060

Parent:

7:acf28eb906c4

Child:

9:831891bb0d11

--- a/main.cpp	Thu Oct 29 11:18:30 2015 +0000
+++ b/main.cpp	Thu Oct 29 17:46:33 2015 +0000
@@ -39,12 +39,12 @@
     DigitalIn   buttonlinks(PTA4);       // button for starting the motor controller
     DigitalIn   buttonrechts(PTC6);     // button for startin calibration procedures
     DigitalIn   reset_button(D1);       // button for returning the arm to the start position
-    DigitalIn   program_button(D0);     // button for starting a preprogrammed movement. (pick up cup)
+    DigitalIn   switch_xy_button(D0);     // button for starting a preprogrammed movement. (pick up cup)
         // init values
         bool loop_start = false;
         bool calib_start = false;
         bool reset = false;
-        bool program = false;
+        bool switch_xy = false;
         
 // LED outputs on bioshield
     DigitalOut led_right(D2);
@@ -59,7 +59,7 @@
 ////////////////////////////////////////////////////////////////////////////////////////////
 
 // switch axes
-    bool switch_xy = false; // bool for switching axes
+    // bool switch_xy = false; // bool for switching axes
     double sw1 = 0; // counter for switching axes
     double t_switch = 0.6; // seconds for switching
     
@@ -117,7 +117,7 @@
     double x_min = 47, x_max = 70, y_min_max = -32;
     double xx,yy,y_min,y_max;
     // Define the maximum rate of change for the x and y reference signals (velocity)
-    double Vmax_x = 7.5, Vmax_y = 15;  // [cm/s]          
+    double Vmax_x = 10, Vmax_y = 15;  // [cm/s]          
 
 
 // CONTROLLER SETTINGS
@@ -158,9 +158,9 @@
             double f1_v1t = 0, f1_v2t = 0, f2_v1t = 0, f2_v2t = 0, f3_v1t = 0, f3_v2t = 0,f4_v1t = 0, f4_v2t = 0;
         
     // Filter coefficients
-        // differential action filter (lowpass 5Hz at 50Hz)
+        // differential action filter (lowpass 5Hz at 200Hz)
             const double m_f_a1 = -1.1430, m_f_a2 = 0.4128, m_f_b0 = 0.0675, m_f_b1 = 0.1349, m_f_b2 = 0.0675;
-        // input filter (lowpass 1Hz at 50samples) (used to make the angle signals smooth)
+        // input filter (lowpass 1Hz at 200Hz) (used to make the angle signals smooth)
             const double r1_f_a1 = -1.6475, r1_f_a2 = 0.7009, r1_f_b0 = 0.0134, r1_f_b1 = 0.0267, r1_f_b2 = 0.0134;    
 
     // EMG-Filter (calculated for 200)
@@ -354,7 +354,7 @@
 // this function is very simple, can be (possibly) improved by writing a single loop.
 void square_move()
 {
-    if (mt1 > 0 && mt1 < T1*controlfreq)     // horizontal movement from (65,-20) -> (55,-20)
+    if (mt1 > 0 && mt1 < T1*controlfreq)     
     {
         xx = x8 + (x1-x8)*(mt1/(T1*controlfreq));
         yy = y8 + (y1-y8)*(mt1/(T1*controlfreq));
@@ -431,10 +431,6 @@
     // update global variables
     output2 = y5t;
     output2_amp = y5t*emg_gain2; 
-    
-    scope.set(0,output1_amp);
-    scope.set(1,output2_amp);
-    scope.send();
 }
  
  
@@ -464,21 +460,27 @@
     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));
+    y_min = - sqrt(5041 - 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));
+    y_max = sqrt(5041 - pow(xx,2));
+    
+    // add x_max (trial) !!!!
+    x_max = sqrt(5041 - pow(yy,2));
+    if (x_max > 70){x_max = 70;}
     
     // use the signal to change the x-reference
     if(switch_xy == false){xx = reference_f(xy_main_input,c_reference_x,x_min,x_max,Vmax_x);}
     // 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 a button has been pressed
-    if(program){square_move();}
+    // start the pre-programmed movement
+    // square_move();
     
-    // check the y-reference (otherwise if x is controlled after y has been controlled, the limits are not followed).
-    if(yy < y_min){yy = y_min;}
-    if(yy > y_max){yy = y_max;}
+    // 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
@@ -510,6 +512,11 @@
         phi_one = reset_phi_one;
         phi_two = reset_phi_two;
     }
+    
+    scope.set(0,yy);
+    scope.set(1,y_min);
+    scope.set(2,y_max);
+    scope.send();
 }
 
 
@@ -617,12 +624,13 @@
             wait(0.2); 
         while(buttonrechts.read() == 0);            
         }
-        // start pre-programmed movement
-        if(program_button.read() == 0)
+        // switch axes
+        if(switch_xy_button.read() == 0)
         {
-           program = !program;  
+           switch_xy = !switch_xy;
+           led_right.write(!led_right.read());
            wait(0.2);  
-           while(program_button.read() == 0);           
+           while(switch_xy_button.read() == 0);           
         }
         if(reset_button.read() == 0)
         {