Diff: ER2_Robot/suivi.cpp

Revision:

0:1a801a2a7b4b

Child:

1:96ef3513d0d5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ER2_Robot/suivi.cpp	Fri Feb 15 15:25:57 2019 +0000
@@ -0,0 +1,79 @@
+#include "mbed.h"
+#include "SRF05.h"
+#include "fonction.h"
+#define ARRET 0
+#define ROULE 1
+#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*/;
+    PWMG.period_us(100);
+    PWMD.period_us(100);
+    int /*J,S,*/etat=ROULE/*,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;
+
+        switch(etat) {
+            case ARRET :                            // A l'arret
+                if(srf>=25) etat=ROULE;
+                break;                       
+            case ROULE :                            // Jack débranché => robot avance
+               if(srf<=25) etat=ARRET; 
+                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);
+                break;
+        }
+
+}
+
+}
+
+