Diff: ER2_Robot/suivi.cpp

Revision:

1:96ef3513d0d5

Parent:

0:1a801a2a7b4b

--- a/ER2_Robot/suivi.cpp	Fri Feb 15 15:25:57 2019 +0000
+++ b/ER2_Robot/suivi.cpp	Fri May 24 10:57:58 2019 +0000
@@ -3,29 +3,19 @@
 #include "fonction.h"
 #define ARRET 0
 #define ROULE 1
+#define DIST 2
 #include "SRF05.h"
 
-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 srf;
 
 int main()
 {
-    float cptG,cptD,E,k=18,EG,ED/*,bat*/;
+    float cptG,cptD,E,k=19,EG,ED/*,bat*/;
     PWMG.period_us(100);
     PWMD.period_us(100);
-    int /*J,S,*/etat=ROULE/*,bpg,bpd*/;
-    BpD.mode(PullUp);
-    BpG.mode(PullUp);
+    int /*J,S,*/etat=ARRET/*,bpg,bpd*/;
+    //BpD.mode(PullUp);
+    //BpG.mode(PullUp);
   
     while(1) {
         
@@ -50,9 +40,13 @@
             case ARRET :                            // A l'arret
                 if(srf>=25) etat=ROULE;
                 break;                       
-            case ROULE :                            // Jack débranché => robot avance
+            case ROULE :  
+                if(25<srf<=29) etat=DIST;                          // Jack débranché => robot avance
                if(srf<=25) etat=ARRET; 
                 break;
+            case DIST :                            // A l'arret
+                if(srf>=25) etat=ROULE;
+                break;   
             
 
 
@@ -70,6 +64,10 @@
                 roule_motdroit(0,(int)(ED));                                  // Roule 
                 printf("Measured : %.1f\n\r", srf);
                 break;
+            case DIST :                           
+                roule_motgauche(0,(int)(EG-10));
+                roule_motdroit(0,(int)(ED-10));
+                break;          
         }
 
 }