Diff: main.cpp

Revision:

28:e6d2fe0e593e

Parent:

27:f62e450bb411

Child:

29:9610df479f89

--- a/main.cpp	Fri Oct 16 12:37:26 2015 +0000
+++ b/main.cpp	Fri Oct 16 13:08:38 2015 +0000
@@ -162,40 +162,40 @@
     scope.set(1, modeR);    
     scope.send();                               //stuur de waardes naar HIDscope
     
-    if(modeR == 1 && !R){                               // R is niet aan
+    if(modeR < 2 && !R){                        // controleer of mode 1 is geweest
         R = true;        
-        }    
-    if(modeL == 1 && !L){                               // L is niet aan
-        L = true;
         }
-            
-    if(modeL>1 && modeR >1) {                           // mode L en mode R beide > 1 
-        R = false;
-        L = false;
-        pwmM2.write(0);                                 // Aim motor stilzetten       
-        aimState = OFF;        
+    if(modeL < 2 && !L){
+        L = true;        
         }
-    else if((modeL == 3)) {                 
+                
+    if((modeL>2) && (modeR >2)) {                       // mode L en mode R beide > 2 
         both = true;                                    // Return true
         pwmM2.write(0);                                 // Aim motor stilzetten
-        aimState = OFF; 
-        L = false;        
-        } 
-    else if(modeR == 3 && aimState!=OFF)  {      
-        R = false;        
-        pwmM2.write(0);                                 // Aim motor stilzetten        
-        aimState = OFF; 
-        PRINT("Motor freeze\r\n");
+        aimState = OFF;        
+        }    
+    else if((modeR == 2) && (modeL == 2)) {              
+        if(aimState!=OFF){
+            pwmM2.write(0);                                 // Aim motor stilzetten        
+            aimState = OFF; 
+            PRINT("Motor freeze\r\n");
+            L = false;
+            R = false;
+            }
         } 
-    else if((modeL == 2) && (aimState != CCW) && L) {       // modeL ==2 AND richting is niet CCW
-        aimState = CCW;                                 // draai CCW
-        pc.printf("Turn negative (CCW)\r\n");
-        motorAim(0);
+    else if((modeL == 2) && (aimState != CCW && (modeR == 1))) {        // modeL ==2 AND richting is niet CCW AND modeR = 1
+        if(L){                    
+            aimState = CCW;                                 // draai CCW
+            pc.printf("Turn negative (CCW)\r\n");
+            motorAim(0);
+            }
         } 
-    else if((modeR == 2) && (aimState != CW) && R) {      // modeR == 2 AND richting is niet CW
-        aimState = CW;                                  // draai CW
-        PRINT("Turn positive (CW)\r\n");
-        motorAim(1);
+    else if((modeR == 2) && (aimState != CW && (modeL == 1))) {         // modeR == 2 AND richting is niet CW AND modeL = 1
+        if(R){
+            aimState = CW;                                  // draai CW
+            PRINT("Turn positive (CW)\r\n");
+            motorAim(1);
+            }
         }    
     return both;            
     }