Diff: main.cpp

Revision:

10:5c5f2ea11045

Parent:

9:350a82a7745a

Child:

11:65d053a1b511

--- a/main.cpp	Thu Oct 29 15:21:45 2015 +0000
+++ b/main.cpp	Fri Oct 30 09:17:33 2015 +0000
@@ -66,9 +66,7 @@
     double EMG_right_f3;
     double EMG_right_abs;
     double Threshold1;
-    double Threshold2;
     double Threshold3;
-    double Threshold4;
     
     int N = 50;
     double MAF_left[50];
@@ -110,6 +108,19 @@
 // General Functions
 //--------------------------------------------------------------------------------------------------------------------------//
 
+//Blinking led
+void BlinkLed(DigitalOut LED, int n, float Time)
+{ 
+    for(int i=1; i<=n;i++)
+    {
+        LED.write(0);
+        wait(Time);
+        LED.write(1);
+        wait(Time);     
+    }
+    LED.write(1);
+}
+
 //HIDScope
     void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
     {
@@ -249,47 +260,23 @@
 //--------------------------------------------------------------------------------------------------------------------------//      
         wait(20);
     
-        LedG.write(1);
-        wait(0.2);
-        LedG.write(0);
-        wait(0.2);
-        LedG.write(1);
-        wait(0.2);
-        LedG.write(0);
-        wait(0.2);
-        LedG.write(1);
-        wait(0.2);    
+        BlinkLed(LedG, 2, 0.2);             //bepaling threshold linkerarm
         LedG.write(0);
         wait(2);
         Threshold1 = 0.5*EMG_left_MAF;
-        Threshold2 = 0.2*EMG_left_MAF;
         LedG.write(1);
-        LedR.write(0);
-        wait(2);
-        LedR.write(1);
+        
+        BlinkLed(LedR, 1, 2);         
 
-        wait(2);
-        LedB.write(1);
-        wait(0.2);
-        LedB.write(0);
-        wait(0.2);
-        LedB.write(1);
-        wait(0.2);
-        LedB.write(0);
-        wait(0.2);
-        LedB.write(1);
-        wait(0.2);    
+        BlinkLed(LedB, 2, 0.2);             //bepaling threshold rechterarm
         LedB.write(0);
         wait(2);
         Threshold3 = 0.5*EMG_right_MAF;
-        Threshold4 = 0.2*EMG_right_MAF;
         LedB.write(1);
-        pc.printf("T1 = %f, T2 = %f, T3 = %f, T4 = %f\n", Threshold1, Threshold2, Threshold3, Threshold4); 
-        LedR.write(0);
-        wait(2);
-        LedR.write(1);
-    
-    
+                
+        pc.printf("Threshold links = %f, Threshold rechts = %f/n", Threshold1, Threshold3); 
+        BlinkLed(LedR, 1, 2);
+        LedR.write(1);    
 //--------------------------------------------------------------------------------------------------------------------------//
 // Control Program
 //--------------------------------------------------------------------------------------------------------------------------//
@@ -318,9 +305,9 @@
     // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED      
         if(count == 0)
         {
-                
                 LedR = LedB = 1;
                 LedG = 0;
+                
                 if ((EMG_right_MAF >= Threshold3) && (EMG_left_MAF <= Threshold1)) //if(c == 'r') //
                 {
                     m2_ref = m2_ref + Stapgrootte;
@@ -395,14 +382,44 @@
     // PROGRAM 2: Firing mechanism & Reset, Blue LED
         if(count == 2) 
         {
-
-                LedR = LedG = 1;
-                LedB = 0;
+                LedG = LedR = 1;
+                pc.printf("Please press the button to shoot the disc\n");
+                BlinkLed(LedB, 25, 0.1);
+                
                 //VUUUUR!! (To Do)
-                wait(1);
-                m2_ref = 0;
-                m1_ref = 0;
-                count = 0;   
+                while (m2_ref != 0)                 // positiearm terug naar startpositie
+                {
+                if(m2_ref < 0)
+                {
+                    m2_ref = m2_ref+Stapgrootte;
+                    wait(0.05);
+                }
+                if(m2_ref > 0)
+                {
+                    m2_ref = m2_ref-Stapgrootte;
+                    wait(0.05);
+                }
+                }
+                
+                while (m1_ref != 0)                     //werparm terug naar startpositie
+                {
+                if(m1_ref<0)
+                {
+                    m1_ref = m1_ref+Stapgrootte;
+                    wait(0.05);
+                }
+                
+                if(m1_ref>0)
+                {    
+                    m1_ref = m1_ref-Stapgrootte;
+                    wait(0.05);
+                }
+                }
+                    
+                if((m2_ref == 0) && (m1_ref == 0))              // terug naar program 0
+                {
+                    count = 0;   
+                }
         }
 
 }