Diff: main.cpp

Revision:

1:4c9994ac229c

Parent:

0:b0e2b38ab272

Child:

2:85ab9173d947

--- a/main.cpp	Tue Oct 27 12:20:03 2015 +0000
+++ b/main.cpp	Tue Oct 27 14:08:23 2015 +0000
@@ -63,6 +63,8 @@
 ////////////////////////////////////////////////////////////////////////////////////////////
 
 double t = 0;       // used for circle movement
+
+double sw1 = 0;
 // physical constants
     const double L = 36;           // lenght of arms
     const double pi = 3.1415926535897;      // pi
@@ -114,10 +116,10 @@
  // 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)
-    double x_min = 47, x_max = 70, y_min_max = -40;
+    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 = 15, Vmax_y = 15;  // [cm/s]          
+    double Vmax_x = 7.5, Vmax_y = 15;  // [cm/s]          
 
 
 // CONTROLLER SETTINGS
@@ -348,6 +350,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();
  }
  
  
@@ -359,11 +366,26 @@
     double xy_input2 = output2_amp;
 
     // use potmeter for debugging purposes (note: does not give a smooth signal due to mechanical breakdown)
-    xy_input1 = potright.read();
-    xy_input2 = potleft.read();
+    //xy_input1 = potright.read();
+    //xy_input2 = potleft.read();
     
     xy_input1 = adapt_signal(xy_input1);
     xy_input2 = adapt_signal(xy_input2);
+    
+    if(xy_input1 > input_threshold && xy_input2 > input_threshold)
+    {
+        sw1++;
+    }
+    if(sw1 == 30)
+    {
+        switch_xy = !switch_xy;     
+        led_right.write(!led_right.read());  // turn on led when switched
+        sw1 = 0;
+    }
+    if(xy_input1 < input_threshold || xy_input2 < input_threshold)
+    {
+        sw1 = 0;
+    }
 
     double xy_main_input = xy_input1 - xy_input2 ;    // subtract inputs to create a signal that can go from -1 to 1
  
@@ -376,9 +398,9 @@
     {
         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(4900 - pow(xx,2));
+        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(4900 - pow(xx,2));
+        y_max = sqrt(5184 - pow(xx,2));
        
     }
     if(switch_xy == true)           // use the signal to change the y-reference
@@ -386,6 +408,7 @@
         yy = reference_f(xy_main_input,c_reference_y,y_min,y_max,Vmax_y);        // change the y-reference
         
     }
+    
     // 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;}
@@ -535,32 +558,32 @@
         if(buttonlinks.read() == 0)
         {
             loop_start = !loop_start;     
-            wait(buttonlinks.read() == 1);
-            wait(0.2);             
+            wait(0.2);
+            while(buttonlinks.read() == 0);             
         }
         if(buttonrechts.read() == 0)
         {
             calib_start = !calib_start;     
-            wait(buttonrechts.read() == 1);
-            wait(0.2);             
+            wait(0.2); 
+        while(buttonrechts.read() == 0);            
         }
         // reverse buttons
         if(switch_xy_button.read() == 0)
         {
            switch_xy = !switch_xy;     
-           wait(switch_xy_button.read() == 1);
            led_right.write(!led_right.read());  // turn on led when switched to y control
-           wait(0.2);             
+           wait(0.2);  
+            while(switch_xy_button.read() == 0);           
         }
         if(reset_button.read() == 0)
         {
             reset = !reset;     
-            wait(reset_button.read() == 1);
             phi_one_curr = phi_one;
             phi_two_curr = phi_two;
             rc1 = 0;
             rc2 = 0;     
             wait(0.2);
+            while(reset_button.read() == 0);
         
         }
         //////////////////////////////////////////////////