Diff: main.cpp

Revision:

45:520d86583ff6

Parent:

44:97f5622db2c4

Child:

46:ed8ada80ba17

--- a/main.cpp	Thu Oct 22 12:14:24 2015 +0000
+++ b/main.cpp	Thu Oct 22 19:59:08 2015 +0000
@@ -136,7 +136,7 @@
     }
 
 void checkSide(AnalogIn& ks, int dir){
-     if((dirM2.read() == dir) && (pwmM2.read()>0)){     
+     if((dirM2.read() == dir) && (pwmM2.read()>0)){        // direction motor clashes with killswitch and motor is on  
         if(ks.read()>0.8){
             pwmM2.write(0);                                // Aim motor freeze
             pc.printf("BOEM! CRASH! KASTUK!\r\n");         // Terminal
@@ -145,7 +145,7 @@
         } 
      }    
       
-void checkAim(){                      
+void checkAim(){            // check if killswitch clashes with direction motor                  
     checkSide(ksLeft, 1);
     checkSide(ksRight, 0);
     }
@@ -169,37 +169,39 @@
         L = true;        
         }
                 
-    if((modeL>2) && (modeR >2 && R && L)) {             // mode L and mode R both 3, and both has been 1 herefore 
+    if((modeL>2 && L) || (modeR >2 && R)) {             // mode L and mode R both 3, and both has been 1 herefore 
         both = true;                                    // Return true
         pwmM2.write(0);                                 // Aim motor freeze
         aimState = OFF;                                 // next state
         }    
     else if((modeR == 2) && (modeL == 2)) {             // modes are both 2
-        if(aimState!=OFF){                              // only if aim motor is rotating
-            pwmM2.write(0);                             // Aim motor freeze       
-            aimState = OFF;                             // motor state is off    
-            pc.printf("Motor freeze\r\n");              // LCD
-            PRINT("Freeze");
-            L = false;                                  // Modes must be first 1 for another action
-            R = false;                                  // "" 
-            }
+        both = true;                                    // Return true
+        pwmM2.write(0);                                 // Aim motor freeze
+        aimState = OFF;                                 // next state
         } 
-    else if((modeR == 2) && (aimState != CCW && (modeL == 1))) {        // modeL ==2 AND rotation is not CCW AND modeR has been 1
-        if(R){                    
+    else if((modeR == 2)) {                             // modeR ==2
+        if(aimState != CCW){                            
             aimState = CCW;                             // Rotate CCW
             pc.printf("Rotate -, CCW\r\n");
             PRINT("Rotate CCW");
-            motorAim(0);
+            motorAim(0);            
             }
         } 
-    else if((modeL == 2) && (aimState != CW && (modeR == 1))) {         // modeR == 2 AND rotation is not CW AND modeL has been 1
-        if(L){
+    else if((modeL == 2)) {                             // modeL == 2
+        if(aimState != CW){
             aimState = CW;                              // Rotate CW
             pc.printf("Rotate +, CW\r\n");
             PRINT("Rotate CW");
             motorAim(1);
             }
-        }    
+        }
+    // modeR AND CCW   OR   modeL and CW
+    else if((modeR == 1 && aimState == CCW) || (modeL == 1 && aimState == CW)) {        // R=1 en CW       
+        pwmM2.write(0);                                 // Aim motor freeze
+        aimState = OFF;                              
+        pc.printf("Freeze\r\n");
+        PRINT("Freeze");                   
+        }      
     return both;            
     }               
 int main(){     
@@ -258,51 +260,23 @@
                 dirM1.write(0);                 // direction beam motor       
                 pwmM1.period(1/100000);         // period beam motor       
                 servo.period(0.02);             // 20ms period servo
-                //pwmM1.write(0.2);             // Turn motor on, low power
+                pwmM1.write(0.1);               // Turn motor on, low power
                 btn = false;                    // Button is unpressed
-                R = false;                      // modeR must become 1
-                L = false;                      // modeL must become 1
                 PRINT("Calibrate beam  to 10 o'clock");
                 wait(1);                
-                PRINT("Flex right half to swing beam");                                  
+                PRINT("Push button to  continue");                                  
                 while(state==CALIBRATE_BEAM){                    
-                    if(emgFlag){                        
+                    if(controlFlag){                        
                         pc.printf("");          // lege regel printen, anders doet setknop het niet
-                        emgFlag = false;
-                        
-                        int modeL = defMode(emgL, potL, true);          // determine modeL
-                        int modeR = defMode(emgR, potR, false);         // determine modeR 
-                        
-                        if(modeR < 2) {           // modeR == 1
-                            R = true;
-                            }
-                        if(modeL < 2) {           // modeL == 1
-                            L = true;
-                            }                                                                                                                   
-                        if (btn || (modeL == 3 && L) || (modeR == 3 && R)){               // If setbutton is on or one mode is 3, and has been 1 
+                        controlFlag = false;                                                                                                                                          
+                        if(btn){               // If setbutton is on or one mode is 3, and has been 1 
                             pwmM1.write(0);                             // Motor 1 freeze
                             enc1.reset();                               // encoder 1 reset
                             PRINT("Beam calibrated");
                             pc.printf("Beam calibrated\r\n");
                             btn = false;                                // button op false
                             state = AIM;                                // next state                          
-                            }
-                        else if(modeL == 2){
-                            if(pwmM1.read()> 0){                        // beam is moving
-                                pwmM1.write(0);                         // beam freeze
-                                PRINT("Relax");                         // LCD
-                                wait(1);
-                                PRINT("Flex both full  to continue");   // LCD
-                                pc.printf("Beam freeze\r\n");
-                                }
-                            }
-                        else if(modeR == 2){
-                            if(pwmM1.read()== 0){                       // beam is freezed 
-                                //pwmM1.write(0.2);                     // beam rotate
-                                PRINT("Flex left half  to stop beam");  // LCD
-                                pc.printf("Beam move\r\n");
-                                }
-                            }
+                            }                        
                         }             
                     }
                 lcd.cls();