Revision 26:e24c82fde126, committed 2020-05-03

Comitter:

burgerking

Date:

Sun May 03 13:59:25 2020 +0000

Parent:

25:f986cbbaf4e8

Commit message:

fghgh;

Changed in this revision

--- a/main.cpp	Sun May 03 12:44:28 2020 +0000
+++ b/main.cpp	Sun May 03 13:59:25 2020 +0000
@@ -53,97 +53,90 @@
 };
 
 int main() { 
-    int iState=Depart;
-    float distObsGauche=pot1.read();
-    float distObsDroite=pot2.read();
-    int SwitchBumpers=bumpers.read();
-    int SwitchUp=up.read();
-    int SwitchJack=jack.read();
-    int SwitchFire=fire.read();   
     lcd.cls();
     lcd.locate(32,16);
     lcd.printf("Module ER2 : template");
     while(true){
-        wait(.5);  
+        int iState=Depart;
+        float distObsGauche=pot1.read();
+        float distObsDroite=pot2.read();
+        int SwitchBumpers=bumpers.read();
+        int SwitchUp=up.read();
+        int SwitchJack=jack.read();
+        int SwitchFire=fire.read(); 
+        int iFlag=0;
+        /*wait(.5);  /*this time depends on the speed of the robot*/
         switch(iState){
-            case Depart : 
-                LED_verte.write(1); 
-                //printf.lcd("Depart");
+            case Depart :
+                if(iFlag==0) 
+                {
+                    LED_verte.write(1); 
+                    LED_rouge.write(0);
+                    initVariateurs();
+                    iFlag=1;
+                }
+                if(SwitchJack==0||SwitchFire==1)
+                    iState=Navigation;
                 break;
+            
             case Navigation : 
+                LED_verte.write(0);
+                LED_rouge.write(0);
                 //No Yellow LED
                 commandeMoteurs(50,50);
-                //printf.lcd("Navigation");
+                if(distObsGauche<0.2||distObsDroite<0.2)/*0.2=20cm,0.4=40cm*/
+                    iState=Rotation;
+                else if((distObsGauche>0.2&&distObsGauche<0.4)||(distObsDroite>0.2&&distObsDroite<0.4))
+                    iState=Evitement;
+                else if(/*white tested||*/SwitchBumpers==1)
+                    iState=Arrive;
                 break;
+            
             case Arrive :
+                wait(.1);
+                LED_rouge.write(0);
                 LED_verte.write(!LED_verte.read());
                 commandeMoteurs(0,0);
-                //printf.lcd("Arrive");
+                if(SwitchUp==1)
+                {
+                    iState=Depart;
+                    iFlag=0;
+                }
                 break;
+            
             case Evitement :
+                wait(.1);
                 LED_rouge.write(1);
+                LED_verte.write(0);
                 if(distObsDroite>distObsGauche)
-                    commandeMoteurs(100,0);
-                    //printf.lcd("Evitement1");
+                    commandeMoteurs(100,0);/*turn right*/
                 else
-                    commandeMoteurs(0,100);
-                    //printf.lcd("Evitement2");
+                    commandeMoteurs(0,100);/*turn left*/
+                if(distObsGauche<0.2||distObsDroite<0.2)
+                    iState=Rotation;
+                else if(distObsGauche>0.4&&distObsDroite>0.4)
+                    iState=Navigation;
+                else if(/*white tested||*/SwitchBumpers==1)
+                iState=Arrive;
                 break;
+            
             case Rotation :
+                LED_verte.write(0);
                 LED_rouge.write(!LED_rouge.read());
                 if(distObsDroite>distObsGauche)
-                    commandeMoteurs(100,0);
-                    //printf.lcd("Rotation1");
+                    commandeMoteurs(100,0);/*turn right*/
                 else
-                    commandeMoteurs(0,100);
-                    //printf.lcd("Rotation2");
+                    commandeMoteurs(0,100);/*turn left*/
+                if(distObsGauche>0.4&&distObsDroite>0.4)
+                    iState=Navigation;
+                if(/*white tested||*/SwitchBumpers==1)
+                    iState=Arrive;
+                if(distObsGauche>0.4&&distObsDroite>0.4)
+                    iState=Navigation;
+                else if(/*white tested||*/SwitchBumpers==1)
+                    iState=Arrive;
                 break;
-            }//switch
-        
-        if(iState==Depart)
-        {
-            if(SwitchJack==0||SwitchFire==1)
-                iState=Navigation;
-            if(SwitchUp==1)
-                iState=Arrive;
-        }
-        
-        if(iState==Navigation)
-        {
-            if(distObsGauche>0.2||distObsDroite>0.2)
-                iState=Rotation;
-            if(0.2<distObsGauche||distObsGauche<0.4&&0.2<distObsDroite||distObsDroite<0.4)
-                iState=Evitement;
-            if(/*white tested||*/SwitchBumpers==1)
-                iState=Arrive;
-        }
+            }//switch        
         
-        if(iState==Arrive)
-        {
-            if(SwitchUp==1)
-                iState=Depart;
-        }
-        
-        if(iState==Evitement)
-        {
-            if(distObsGauche<0.2||distObsDroite<0.2)
-                iState=Rotation;
-            if(distObsGauche>0.4&&distObsDroite>0.4)
-                iState=Navigation;
-            if(/*white tested||*/SwitchBumpers==1)
-                iState=Arrive;
-        }
-        
-        if(iState==Rotation)
-        {
-            if(distObsGauche>0.4&&distObsDroite>0.4)
-                iState=Navigation;
-            if(/*white tested||*/SwitchBumpers==1)
-                iState=Evitement;
-        }
-            
-            
-            
-
-    }//while
+        }//while
   }  
\ No newline at end of file