Diff: main.cpp

Revision:

0:a8cee96c9250

Child:

1:99f469d63b9a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Jun 10 04:58:21 2017 +0000
@@ -0,0 +1,135 @@
+#include "mbed.h"
+#include "fct.h"
+#include "Pixy.h"
+#include "PixyLink.h"
+#include "SRF05.h"
+#include <CMPS03.h>
+
+Serial pc (USBTX,USBRX);
+int main()
+{
+    init();
+    Pixy pixy= Pixy(p28,p27);
+    pixy.setSerialOutput(&pc);
+    pc.printf("alive");
+
+    while(1) {
+        lecture_blanc();    // recupere valeurs capteurs ligne blanche
+        lecture_us();   // recupere valeurs capteurs us
+        lecture_boussole(); //gBoussole
+        bout=bp.read();
+        blocks = pixy.getBlocks();
+        //pc.printf("etat=%d\n\r",etat);
+        //pc.printf("etat=%d,capt1= %d, capt2= %d, us_ar=%f, bp=%d, boussole=%f\n\r",etat,captL1,captL3,us_arriere,bp.read(),gBoussole);
+        if (blocks) {
+            taille=pixy.blocks[j].width*pixy.blocks[j].height;
+            errorX = (160-pixy.blocks[j].x);
+            //pc.printf("sig: %d x: %d y: %d width: %d height: %d\n\r", pixy.blocks[j].signature, pixy.blocks[j].x, pixy.blocks[j].y, pixy.blocks[j].width, pixy.blocks[j].height);
+            //pc.printf("OK");
+        }
+
+        switch(etat)
+        {
+            case(0) :
+            
+                if(bout==0)
+                {
+                    etat=100;
+                    cap_set=gBoussole;
+                }
+                break;
+            
+            case(100):
+            
+                if(blocks)
+                {
+                    etat=1;
+                }
+                
+                else
+                {
+                    etat=21;
+                }
+                break;
+                 
+            case(1) :
+            
+                if(!blocks)
+                {
+                    etat=2;
+                    tempo.stop();
+                    tempo.reset();
+                    tempo.start();
+                }
+                break;
+            case(2) :
+            
+                if(tempo.read()>0.5)
+                {
+                    etat=3;
+                }
+                break;
+            case(3) :
+            
+                if(us_arriere<25)
+                {
+                    etat=100;
+                }
+                break;
+            case(21) :
+                cap_rot=cap_set+30;
+                if(cap_rot>360)
+                {
+                    cap_rot=cap_rot-360;
+                }
+                
+                if((gBoussole>cap_rot)&&(gBoussole<cap_rot+180))
+                {
+                    etat=22;
+                }
+                if(blocks)
+                {
+                    etat=1;
+                }
+                break;
+            case(22) :
+                cap_rot=cap_set-30;
+                if(cap_rot<0)
+                {
+                    cap_rot=cap_rot+360;
+                }
+                
+                if((gBoussole<cap_rot)&&(gBoussole>cap_rot-180))
+                {
+                    etat=21;
+                }
+                if(blocks)
+                {
+                    etat=1;
+                }
+                break;
+            
+        }
+        
+        switch(etat)
+        {
+            case(0) : vitmoteur(0,0);break;
+            
+            case(1) : vitmoteur(V_max-K*errorX,V_max+K*errorX);break;
+            
+            case(2) : vitmoteur(V_max,V_max);break;
+            
+            case(3) : vitmoteur(-V_moy,-V_moy);break;
+            
+            case(21) : vitmoteur(V_recherche,-V_recherche);break;           
+            
+            case(22) : vitmoteur(-V_recherche,V_recherche);break;
+            
+            case(100) : break;
+        } 
+       
+    }
+}
+        
+
+