Diff: ER2_Robot/suivi.cpp

Revision:

2:73e8dca28f51

Parent:

1:96ef3513d0d5

Child:

3:b85df47a059a

--- a/ER2_Robot/suivi.cpp	Fri May 24 10:57:58 2019 +0000
+++ b/ER2_Robot/suivi.cpp	Mon Jun 17 11:12:06 2019 +0000
@@ -3,74 +3,74 @@
 #include "fonction.h"
 #define ARRET 0
 #define ROULE 1
-#define DIST 2
-#include "SRF05.h"
+
 
-float srf;
+SRF05 SRF(p13,p14);
+DigitalOut SensG(p12);//Sens Gauche
+DigitalOut SensD(p11);//Sens Droite
+PwmOut PWMG(p23); //Sortie moteur gauche
+PwmOut PWMD(p22); //Sortie moteur droit
+DigitalIn BpD(p29);
+DigitalIn BpG(p30);
+AnalogIn CPTG (p20);
+AnalogIn CPTD (p19);
+AnalogIn BAT (p15);
+
+float srfa,srfc;
 
 int main()
 {
-    float cptG,cptD,E,k=19,EG,ED/*,bat*/;
+
     PWMG.period_us(100);
     PWMD.period_us(100);
-    int /*J,S,*/etat=ARRET/*,bpg,bpd*/;
-    //BpD.mode(PullUp);
-    //BpG.mode(PullUp);
-  
+    int /*J,S,*/etat=0/*,bpg,bpd*/;
+    BpD.mode(PullUp);
+    BpG.mode(PullUp);
+
     while(1) {
-        
-        wait(0.01);
-        //bat=BAT.read()*4*3.3;
-        //bat=7/bat;
-        //J=Jack.read();
-        //S=Switch.read();
-                cptG=CPTG.read();          // Lis les valeurs capteurs
-                cptD=CPTD.read();          // Lis les valeurs capteurs
-                srf=SRF.read();
-    
-       E=cptG-cptD;
-       EG=40/**bat*/+k*E;
-       ED=40/**bat*/-k*E;
-       if(EG<0) EG=0;
-       if(EG>100) EG=100;
-       if(ED<0) ED=0;
-       if(ED>100) ED=100;
+        srfa=SRFA.read();
+        srfc=SRFC.read();
 
         switch(etat) {
-            case ARRET :                            // A l'arret
-                if(srf>=25) etat=ROULE;
-                break;                       
-            case ROULE :  
-                if(25<srf<=29) etat=DIST;                          // Jack débranché => robot avance
-               if(srf<=25) etat=ARRET; 
+            case 0 :
+                if(srfc<=5) etat=2;
+                if(srfc>=10) etat=1;
+                if(srfc<=5 && srfa<=5) etat=3;
                 break;
-            case DIST :                            // A l'arret
-                if(srf>=25) etat=ROULE;
-                break;   
-            
-
-
+            case 1 : //ZIG
+                if(srfc<=5) etat=2;
+                if(srfc<=5 && srfa<=5) etat=3;
+                break;
+            case 2 : //ZAG
+                if(srfc>=10) etat=1;
+                if(srfc<=5 && srfa<=5) etat=3;
+                break;
+            case 3 ://MUR
+                if(srfa>=10) etat=0;
+                break;
         }
 
         switch(etat) {
-            
-            case ARRET :                            // A l'arret
-                roule_motdroit(0,0);
-                roule_motgauche(0,0);
-                break;                     
-            
-            case ROULE :
-                roule_motgauche(0,(int)(EG));
-                roule_motdroit(0,(int)(ED));                                  // Roule 
-                printf("Measured : %.1f\n\r", srf);
+
+            case 0 :
+                roule_motgauche(0,60);
+                roule_motdroit(0,60);                                  // Roule
+                break;
+            case 1 :
+                roule_motgauche(0,65);
+                roule_motdroit(0,60);
                 break;
-            case DIST :                           
-                roule_motgauche(0,(int)(EG-10));
-                roule_motdroit(0,(int)(ED-10));
-                break;          
+            case 2 :
+                roule_motgauche(0,60);
+                roule_motdroit(0,65);
+                break;
+            case 3 :
+                roule_motgauche(0,50);
+                roule_motdroit(0,70);
+                break;
         }
 
-}
+    }
 
 }