Diff: main.cpp

Revision:

7:2f4660e9cf92

Parent:

6:5f7df5c74a77

--- a/main.cpp	Wed May 31 08:48:23 2017 +0000
+++ b/main.cpp	Fri Jun 23 11:19:58 2017 +0000
@@ -1,60 +1,245 @@
 #include "mbed.h"
 #include "fct.h"
-BusOut ledsetat(p12,p13);
+#include "Pixy.h"
+#include "PixyLink.h"
+#include "CMPS03.h"
+
+
+Serial pc(USBTX,USBRX);
+CMPS03 compass(p9, p10, CMPS03_DEFAULT_I2C_ADDRESS);
 
 int main()
 {
-    int etat=0;
+
     init();
-    while(1) {
-       // printf("etat=%d US1=%.0f US2=%.0f US3=%.0f erreur=%.0f AN1=%.0f AN2=%.0f cmdD=%.0f cmdG=%.0f\n\r",etat,US1,US2,US3,(US2-US1),AN1,AN2,cmdD,cmdG);
-        //wait(0.05);
-        ledsetat.write(etat);
+    Pixy pixy = Pixy(p28,p27);
+    pixy.setSerialOutput(&pc);
+    int tempo=0;
+    int i=0;
+    while(true) {
+        //capg=capb1.read();
+        //capd=capb2.read();
+        capg=0;
+        capd=0;
+
+        //i++;pc.printf("%d\n\r",i);
+        jck=jack.read();
+        leds.write(etat);
+        //pc.printf("g=%d,d=%d\n\r",capd,capg);
+        //pc.printf("etat=%d,taille=%f,capt_eg=%3.0f,capt_g=%3.0f,capt_m=%3.0f,capt_d=%3.0f,capt_ed=%3.0f\n\r",etat,taille,capt_eg,capt_g,capt_m,capt_d,capt_ed);
+        //pc.printf("etat=%d, cap_lum=%f, cap=%f, block=%d,taille=%f\n\r",etat,cap_lum,cap,blocks,taille);
+        cap=compass.readBearing()/10.0;
+        blocks = pixy.getBlocks();
+        if (blocks) {
+            taille=pixy.blocks[j].width*pixy.blocks[j].height;
+            errorX = (160-pixy.blocks[j].x);
+            errorY = (100-pixy.blocks[j].y);
+        } /*else if(!blocks) {
+            blocks = pixy.getBlocks();
+            taille=pixy.blocks[j].width*pixy.blocks[j].height;
+            errorX = (160-pixy.blocks[j].x);
+            errorY = (100-pixy.blocks[j].y);
+        }*/
+        if(tr.read()>10) {
+            tempo=1;
+        }
+
+        //pc.printf("taille=%f\n\r",taille);
         switch(etat) {
-            case 0 :
-                if((AN2>10 && AN2<20) || US3<13) {
+            case(0) :
+                if(jck==1) {
                     etat=1;
-                    stopMotor();
+                    cap_lum=cap;
+                    tr.stop();
+                    tr.reset();
+                    tr.start();
+                }
+                break;
+
+            case(1) :
+
+                cap_lum=cap;
+                if(!blocks) {
+                    etat=3;
+                }
+                if((capt_eg<Dist_limit)||(capt_g<Dist_limit)||(capt_m<Dist_limit)||(capt_d<Dist_limit)||(capt_ed<Dist_limit)) {
+                    etat=2;
                 }
-                if(US2>100 && US1<40) {
-                    etat=2;
-                    stopMotor();
+
+
+                if((taille>seuil_taille)&&(tempo==1)) {
+                    tb.reset();
+                    tb.start();
+                    etat=100;
+                }
+                break;
+
+            case(2) :
+                if((capt_eg>Dist_limit)&&(capt_g>Dist_limit)&&(capt_m>Dist_limit)&&(capt_d>Dist_limit)&&(capt_ed>Dist_limit)&&(blocks))  {
+                    etat=1;
+                }
+                if((capt_eg>Dist_limit)&&(capt_g>Dist_limit)&&(capt_m>Dist_limit)&&(capt_d>Dist_limit)&&(capt_ed>Dist_limit)&&(!blocks))  {
+
+                    etat=3;
+                }
+
+                if((taille>seuil_taille)&&(tempo==1)) {
+                    tb.reset();
+                    tb.start();
+                    etat=100;
                 }
                 break;
-            case 1 :
-                if(US2>100 && US1<40) {
+
+
+            case(3) :
+                if(cap_lum<180) {
+                    if(cap<=cap_lum+180 && cap>cap_lum) {
+                        etat=5;
+                    } else {
+                        etat=4;
+                    }
+                } else {
+                    if(cap<=cap_lum && cap>cap_lum-180) {
+                        etat=4;
+                    }
+
+                    else {
+                        etat=5;
+                    }
+                }
+
+                if((taille>seuil_taille)&&(tempo==1)) {
+                    tb.reset();
+                    tb.start();
+                    etat=100;
+                }
+                break;
+
+            case(4) : //pc.printf("tourner a gauche");
+
+                if (blocks) {
+                    etat=1;
+                }
+                if((capt_eg<Dist_limit)||(capt_g<Dist_limit)||(capt_m<Dist_limit)||(capt_d<Dist_limit)||(capt_ed<Dist_limit)) {
                     etat=2;
-                    stopMotor();
+                }
+
+                if((taille>seuil_taille)&&(tempo==1)) {
+                    tb.reset();
+                    tb.start();
+                    etat=100;
                 }
-                if(AN2>30) {
-                    etat=0;
-                    t2.start();
-                    t2.reset();
-                    stopMotor();
+                break;
+
+            case(5) :
+
+                //pc.printf("touner a droite");
+                if (blocks) {
+                    etat=1;
+                }
+                if((capt_eg<Dist_limit)||(capt_g<Dist_limit)||(capt_m<Dist_limit)||(capt_d<Dist_limit)||(capt_ed<Dist_limit)) {
+                    etat=2;
+                }
+
+                if((taille>seuil_taille)&&(tempo==1)) {
+                    tb.reset();
+                    tb.start();
+                    etat=100;
+                }
+                break;
+
+            case(10) :
+
+                if(capt_m>=40) {
+                    etat=2;
+                }
+                break;
+
+
+            case(100) :
+
+                if((capt_m<30) || (capt_g<25)) {
+                    etat=101;
+                    t.stop();
+                    t.reset();
+                    t.start();
+                }
+                break;
+
+            case(101) :
+                if(t.read()<10) {
+                    servo_start();
+                } else if(t.read()>=10) {
+                    servo_stop();
                 }
 
                 break;
-            case 2 :
-                if((AN2>10 && AN2<20) || US3<13) {
-                    etat=1;
-                    stopMotor();
-                }
-                if(t2.read()>2.5) {
-                    etat=0;
-                }
-                break;
+
+
         }
         switch(etat) {
-            case 0 :
-                suivi_mur();
+            case(0) :
+                vitesse(0,0);
+                break;
+
+            case(1) :
+                vitesse(V_max-K*errorX,V_max+K*errorX);
                 break;
-            case 1 :
-                rotation_horaire();
+            case(2) :
+                erreur_cap=(cap_lum+(80-(capt_eg+capt_g+capt_m)/3)*Kdist-(80-(capt_ed+capt_d)/2)*Kdist)-cap;
+                if(erreur_cap>180) {
+                    erreur_cap=erreur_cap-360;
+                }
+                if(erreur_cap<-180) {
+                    erreur_cap=erreur_cap+360;
+                }
+                if(erreur_cap>0) {
+                    cmdD=V_max-Kcap*erreur_cap;
+                    cmdG=V_max;
+                } else {
+                    cmdD=V_max;
+                    cmdG=V_max+Kcap*erreur_cap;
+                }
+                if(cmdD<0) {
+                    cmdD=0;
+                }
+                if(cmdG<0) {
+                    cmdG=0;
+                }
+                vitesse(cmdG,cmdD);
+                break;
+            case(3) :
                 break;
-            case 2 :
-                contournement();
+
+            case(4) :
+                vitesse(V_moy,lent);
+                break;
+
+            case(5) :
+                vitesse(lent,V_moy);
+                break;
+
+            case(10):
+                vitesse(lent,lent);
                 break;
+
+            case(100) :
+
+                vitesse(V_max-K*errorX,V_max+K*errorX);
+                break;
+
+            case(101) :
+
+                vitesse(0,0);
+                break;
+            case(111) :
+                vitesse(-30,0);
+                break;
+
+            case(112) :
+                vitesse(0,-30);
+                break;
+
         }
-
     }
 }