Diff: main.cpp

Revision:

29:0f1c25496bdc

Parent:

28:2eb768b9cb3b

--- a/main.cpp	Mon Oct 19 09:22:05 2015 +0000
+++ b/main.cpp	Mon Oct 19 09:55:33 2015 +0000
@@ -105,7 +105,7 @@
         scope.set(0, reference2 - position2);
         scope.set(1, position2);
         scope.set(2, EMG_left_MAF);    
-        scope.set(3, EMG_left);
+        scope.set(3, EMG_right_MAF);
         scope.send();
     
     }
@@ -260,55 +260,61 @@
         Threshold1 = 0.5*EMG_left_MAF;
         Threshold2 = 0.2*EMG_left_MAF;
         LedG.write(1);
+        LedR.write(0);
+        wait(2);
+        LedR.write(1);
+
+        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);    
+        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("T1 = %f, T2 = %f\n", Threshold1, Threshold2); 
-        LedG.write(1);
     
 //--------------------------------------------------------------------------------------------------------------------------//
 // Control Program
 //--------------------------------------------------------------------------------------------------------------------------//
     while(true)
     {
-        char c = pc.getc();
-    // 1 Program UP
-        if(c == 'e') 
-        {
-            count = count + 1;
-            if(count > 2)
-                {
-                    count = 2;
-                }
-
-        }
-     // 1 Program DOWN
-        if(c == 'd')
-        {
-            count = count - 1;
-            if(count < 0)
-                {
-                    count = 0;
-                }
-        }
     // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED      
-        if(count == 0)
+        if(true)
         {
                 
                 LedR = LedB = 1;
-                LedG = 0;
+                LedG = 1;
                 if(EMG_left_MAF > Threshold1)
                 {
                     m2_ref = m2_ref + Stapgrootte;
                     m1_ref = m1_ref - Stapgrootte;
+                    wait(0.1);
+                    LedG = 0;
                     if (m2_ref > Grens2)
                     {
                         m2_ref = Grens2;
                         m1_ref = -1*Grens1;
                     }
                 }
-                if(c == 'f')
+                if(EMG_right_MAF > Threshold3)
                 {
                     m2_ref = m2_ref - Stapgrootte;
                     m1_ref = m1_ref + Stapgrootte;
+                    wait(0.1);
+                    LedB = 0;
                     if (m2_ref < -1*Grens2)
                     {
                         m2_ref = -1*Grens2;
@@ -316,39 +322,5 @@
                     }
                 }
         }
-    // PROGRAM 1: Motor 1 control, Red LED
-        if(count == 1) 
-        {
-                LedG = LedB = 1;
-                LedR = 0;
-                if(c == 't')
-                {
-                    m1_ref = m1_ref + Stapgrootte;
-                    if (m1_ref > Grens1)
-                    {
-                        m1_ref = Grens1;
-                    }
-                }
-                if(c == 'g')
-                {
-                    m1_ref = m1_ref - Stapgrootte;
-                    if (m1_ref < -1*Grens1)
-                    {
-                        m1_ref = -1*Grens1;
-                    }
-                }
-        }
-    // PROGRAM 2: Firing mechanism & Reset, Blue LED
-        if(count == 2) 
-        {
-
-                LedR = LedG = 1;
-                LedB = 0;
-                //VUUUUR!! (To Do)
-                wait(1);
-                m2_ref = 0;
-                m1_ref = 0;   
-        }
     }
-
 }
\ No newline at end of file