Diff: main.cpp

Revision:

15:5d24f832bb7b

Parent:

14:95acac6a07c7

Child:

16:2cf8c2705936

--- a/main.cpp	Thu Nov 02 12:19:54 2017 +0000
+++ b/main.cpp	Thu Nov 02 13:05:59 2017 +0000
@@ -35,6 +35,8 @@
 DigitalOut led1(LED_RED);
 DigitalOut led2(LED_BLUE);
 DigitalOut led3(LED_GREEN);
+DigitalOut led4(D8);                                                            // CONNECT LED1 TO D8
+DigitalOut led5(D9);                                                            // CONNECT LED2 TO D9
 AnalogIn emg_r(A2);                                                             // CONNECT EMG TO A2
 AnalogIn emg_l(A3);                                                             // CONNECT EMG TO A3
  
@@ -108,6 +110,9 @@
  
 Ticker sampleTicker; 
 const double sampleTs = 0.05;
+
+volatile bool xdir = true, ydir = false;
+volatile bool timer = false;
  
 // FUNCTIONS ///////////////////////////////////////////////////////////////////
 // BIQUAD FILTER FOR DERIVATIVE OF ERROR ///////////////////////////////////////
@@ -353,31 +358,34 @@
         position1 = radPerPulse * Encoder1.getPulses();
         position2 = radPerPulse * Encoder2.getPulses();
         
-        //check_emg_r(), check_emg_l();
-        
-        if(active_r && reference1 > motor1Min && reference2 < motor2Max) 
+        if(xdir)
         {
-            xVelocity = velocity;
-            //pc.printf("button1 \r\n");
-        }
-        else if(active_l && reference1 < motor1Max && reference2 > motor2Min)
-        {
-            xVelocity = -velocity;
-            //pc.printf("   button2 \r\n");
+            if(active_r && reference1 > motor1Min && reference2 < motor2Max) 
+            {
+                xVelocity = velocity;
+                pc.printf("x positive \r\n");
+            }
+            else if(active_l && reference1 < motor1Max && reference2 > motor2Min)
+            {
+                xVelocity = -velocity;
+                pc.printf("x negative \r\n");
+            }
+            else xVelocity = 0;
         }
-        else xVelocity = 0;
-     
-        if(!button3 && reference2 < motor2Max )//&& reference1 > motor2Min)
+        else if(ydir)
         {
-            yVelocity = velocity;
-            //pc.printf("      button3 \r\n");
+            if(active_r && reference2 < motor2Max )//&& reference1 > motor2Min)
+            {
+                yVelocity = velocity;
+                pc.printf("y positive \r\n");
+            }
+            else if(active_l && reference2 > motor2Min )//&& reference1 > motor2Min)
+            {
+                yVelocity = -velocity;
+                pc.printf("y negative \r\n");
+            }
+            else yVelocity = 0;
         }
-        else if(!button4 && reference2 > motor2Min )//&& reference1 > motor2Min)
-        {
-            yVelocity = -velocity;
-            //pc.printf("         button4 \r\n");
-        }
-        else yVelocity = 0;
         
         //pc.printf("x %f, y %f \r\n", xVelocity, yVelocity);
         
@@ -450,7 +458,6 @@
         else if(reference2 + msh[1]*sampleTs < motor2Min) reference2 = motor2Min;
         else reference2 = reference2 + msh[1]*sampleTs;
         
-        
         scope.set(0,reference1);
         scope.set(1,position1);
         scope.set(2,reference2);
@@ -463,6 +470,13 @@
         
     }
 }
+
+void StartTimer()
+{
+    timer = true;
+    wait(3);
+    timer = false;
+}
  
 // STATES //////////////////////////////////////////////////////////////////////
 void ProcessStateMachine()
@@ -474,8 +488,8 @@
             // State initialization
             if(stateChanged)
             {
-                //pc.printf("Entering MOTORS_OFF \r\n"
-                //"Press button 1 to enter CALIBRATING \r\n");
+                pc.printf("Entering MOTORS_OFF \r\n"
+                "Press button 1 to enter CALIBRATING \r\n");
                 TurnMotorsOff();                                                // Turn motors off
                 stateChanged = false;
             }
@@ -495,8 +509,8 @@
             // State initialization
             if(stateChanged)
             {
-                //pc.printf("Entering CALIBRATING \r\n"
-                //"Press button 1 to enter MOVING \r\n");
+                pc.printf("Entering CALIBRATING \r\n"
+                "Press button 1 to enter MOVING \r\n");
                 stateChanged = false;
                 calibration_r();
                 calibration_l();
@@ -520,11 +534,27 @@
             // State initialization
             if(stateChanged)
             {
-                //pc.printf("Entering MOVING \r\n");
+                pc.printf("Entering MOVING \r\n");
                 //"Press button 2 to enter HITTING \r\n");
                 stateChanged = false;
             }
             
+            if(active_l && active_r)
+            {
+                if(!timer)
+                {
+                    active_l = false;
+                    active_r = false;
+                    xdir = !xdir;
+                    ydir = !ydir;
+                    led4 = !led4;
+                    led5 = !led5;
+                    if(active_l)pc.printf("links active");
+                    else pc.printf("links inactive");
+                    StartTimer();
+                }
+            }
+            
             // Hit command    
             /*if(!button2)
             {
@@ -568,6 +598,9 @@
     led1.write(1);
     led2.write(1);
     led3.write(1);
+    led4.write(1);
+    led5.write(0);
+    
     pc.printf("START \r\n");
     
     bqc.add( &bq1_low ).add( &bq2_high ).add( &bq3_notch );