Diff: main.cpp

Revision:

20:fa411d58e3ee

Parent:

19:c3d287239c9c

Child:

21:39815fb7e4f5

--- a/main.cpp	Sat May 02 13:19:01 2020 +0000
+++ b/main.cpp	Sun May 03 06:36:00 2020 +0000
@@ -53,7 +53,13 @@
 };
 
 int main() { 
-    int iState=Depart;   
+    int iState=Depart;
+    int distObsGauche=pot1.write();
+    int distObsDroite=pot2.write();
+    int bumpers=bumpers.write();
+    int up=up.write();
+    int jack=jack.write();
+    int fire=fire.write();   
     lcd.cls();
     lcd.locate(32,16);
     lcd.printf("Module ER2 : template");
@@ -62,11 +68,82 @@
         switch(iState){
             case Depart : 
                 LED_verte.write(1); 
+                //printf.lcd("Depart");
                 break;
             case Navigation : 
+                //No Yellow LED
+                commandeMoteurs(50,50);
+                //printf.lcd("Navigation");
+                break;
             case Arrivee :
+                LED_verte.write(!LED_verte.read());
+                commandeMoteurs(0,0);
+                //printf.lcd("Arrive");
+                break;
             case Evitement :
+                LED_rouge.write(1);
+                if(distObsDroite>distObsGsuche)
+                    comandeMoteur(100,0);
+                    //printf.lcd("Evitement1");
+                else
+                    commandeMoteur(0,100);
+                    //printf.lcd("Evitement2");
+                break;
             case Rotation :
+                LED_rouge.write(!LED_rouge.read());
+                if(distObsDroite>distObsGsuche)
+                    comandeMoteur(100,0);
+                    //printf.lcd("Rotation1");
+                else
+                    commandeMoteur(0,100);
+                    //printf.lcd("Rotation2");
+                break;
             }//switch
+        
+        while(iState=Depart)
+        {
+            if(jack==0||fire==1)
+                iState=Navigation;
+            if(up==1)
+                iState=Arrivee;
+        }
+        
+        while(iState=Navigation)
+        {
+            if(//distObsGauche>20cm||distObsDroite>20cm)
+                iState=Rotation;
+            if(//20cm<distObsGauche<40cm&&20cm<distObsDroite<0cm)
+                iState=Evitation;
+            if(/*white tested*/||bumpers==1)
+                iState=Arrivee;
+        }
+        
+        while(iState=Arrivee)
+        {
+            if(up==1)
+                iState=Depart;
+        }
+        
+        while(iState=Evitement)
+        {
+            if(//distObsGauche<20cm||distObsDroite<20cm)
+                iState=Rotation;
+            if(//distObsGauche>40cm&&distObsDroite>40cm)
+                iState=Navigation;
+            if(/*white tested*/||bumpers==1)
+                iState=Arrivee;
+        }
+        
+        while(iState=Rotation)
+        {
+            if(//distObsGauche>40cm&&distObsDroite>40cm)
+                iState=Navigation;
+            if(/*white tested*/||bumpers==1)
+                iState=Evitement;
+        }
+            
+            
+            
+
     }//while
   }  
\ No newline at end of file