main.cpp

00001 #include "mbed.h"
00002 #include "CMPS03.h"
00003 #include "CNY70.h"
00004 #include "GP2A.h"
00005 #include "RC_Servo.h"
00006 #include "VMA306.h"
00007 #include "Pixy.h"
00008 #include "PID.h"
00009 
00010 #define PI  3.1415926535898
00011 
00012 #define NSpeed  100
00013 
00014 Serial      pc          (PA_2, PA_3, 921600);
00015 PID         motor       (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);
00016 
00017 CMPS03      boussole    (PC_4);
00018 
00019 CNY70       ligneD      (PC_3);
00020 CNY70       ligneG      (PC_2);
00021 CNY70       exterior    (PA_7);
00022 
00023 GP2A        ld1         (PA_4, 20, 150, 55);
00024 GP2A        sd1         (PB_0, 5, 80, 10);
00025 
00026 RC_Servo    ballon      (PB_10, 0);
00027 RC_Servo    verrou      (PA_15, 0);
00028 
00029 VMA306      UltraSon    (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2);
00030 
00031 PIXY        Pixy        (PA_0, PA_1, 230400);
00032 
00033 DigitalIn   bp          (PC_13);
00034 
00035 DigitalOut  led1        (PA_5);
00036 DigitalOut  led2        (PD_2);
00037 DigitalOut  disquette   (PA_12);
00038 
00039 main ()
00040 {
00041     int                 etatmvt = 0;
00042     int                 sens = 1, nbiter = 0, nbCC, nbNM;
00043     double              pos = 0.5;
00044     T_pixyCCBloc        CCBuf;
00045     T_pixyNMBloc        NMBuf;
00046     double              x, y, theta, vG, vD;
00047 
00048     pc.printf ("\n\rHelloWorld\n");
00049     led1 = 1;
00050     led2 = 0;
00051     disquette = 0;
00052     
00053     //motor.resetPosition();
00054 
00055     wait (5);
00056 
00057     while (1) {
00058         pc.printf ("\rCap = %5.2lf\n", boussole.getBearing());
00059 
00060         if (exterior.whatAmIOn())   pc.printf("\rwhite");
00061         else                        pc.printf("\rblue ");
00062         if (ligneG.whatAmIOn())     pc.printf("\r\twhite");
00063         else                        pc.printf("\r\tblue ");
00064         if (ligneD.whatAmIOn())     pc.printf("\r\t\twhite\n");
00065         else                        pc.printf("\r\t\tblue\n");
00066 
00067         pc.printf ("\r%4.3lf\t %4.3lf\t %4.3lf\n", exterior.getVoltage(), ligneG.getVoltage(), ligneD.getVoltage());
00068 
00069         pc.printf("\rGP2 longue distance = %5.2f\n",ld1.getDistance ());
00070         pc.printf("\rGP2 courte distance = %5.2f\n",sd1.getDistance ());
00071 
00072         pc.printf("\rpos = %2.1lf\n",pos);
00073         ballon.write (pos);
00074         verrou.write (pos);
00075         nbiter++;
00076         if (nbiter%5==0) {
00077             if (sens) {
00078                 pos += 0.1;
00079                 if (pos>0.9) sens = 0;
00080             } else {
00081                 pos-=0.1;
00082                 if (pos<0.1) sens = 1;
00083             }
00084         }
00085 
00086         if (UltraSon.isUSGReady())     pc.printf ("\rusG = %5.2lf -", UltraSon.readUSG());
00087         if (UltraSon.isUSBReady())     pc.printf ("\r\t\t usB = %5.2lf -", UltraSon.readUSB());
00088         if (UltraSon.isUSDReady())     pc.printf ("\r\t\t\t\t usD = %5.2lf", UltraSon.readUSD());
00089         pc.printf ("\n");
00090 
00091         if (Pixy.checkNewImage()) {
00092             Pixy.detectedObject (&nbNM, &nbCC);
00093             pc.printf ("\rnbCC = %2d - nbNM = %2d\n", nbCC, nbNM);
00094             while (nbCC > 0) {
00095                 CCBuf = Pixy.getCCBloc ();
00096                 nbCC--;
00097                 pc.printf ("\rCC %5d : x=%5d, y=%5d - w=%5d, h=%5d, a=%5d\n", CCBuf.signature, CCBuf.x, CCBuf.y, CCBuf.width, CCBuf.height, (short)CCBuf.angle);
00098             }
00099             while (nbNM > 0) {
00100                 NMBuf = Pixy.getNMBloc ();
00101                 nbNM--;
00102                 pc.printf ("\rNM %4x : x=%5d, y=%5d - w=%5d, h=%5d\n", NMBuf.signature, NMBuf.x, NMBuf.y, NMBuf.width, NMBuf.height);
00103             }
00104         }
00105 
00106         // Square danse !!!
00107         motor.getPosition (&x,&y, &theta);
00108         motor.getSpeed (&vG, &vD);
00109 
00110         pc.printf ("\rEtape = %d : x = %5.1lf, y = %5.1lf, theta = %5.1lf - speedG = %5.1lf, speedD = %5.1lf\n", etatmvt, x, y, 180*theta/PI, vG, vD);
00111 
00112         switch (etatmvt) {
00113             case 0 :
00114                 // On avance de 50cm (coordonnés X = 500, Y = 0)
00115                 motor.setSpeed (NSpeed,NSpeed);
00116                 if (x > 300) {
00117                     etatmvt = 1;
00118                 }
00119                 break;
00120             case 1 :
00121                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2
00122                 motor.setSpeed (NSpeed,-NSpeed);
00123                 if (theta < (-PI/2.0)) {
00124                     etatmvt = 2;
00125                 }
00126                 break;
00127             case 2 :
00128                 // On avance de 50cm (coordonnés X = 500, Y = -500)
00129                 motor.setSpeed (NSpeed,NSpeed);
00130                 if (y < -300)  {
00131                     etatmvt = 3;
00132                 }
00133                 break;
00134             case 3 :
00135                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage)
00136                 motor.setSpeed (NSpeed,-NSpeed);
00137                 if (theta > 0)  {
00138                     etatmvt = 4;
00139                 }
00140                 break;
00141             case 4 :
00142                 // On avance de 50cm (coordonnés X = 0, Y = -500)
00143                 motor.setSpeed (NSpeed,NSpeed);
00144                 if (x < 0)  {
00145                     etatmvt = 5;
00146                 }
00147                 break;
00148             case 5 :
00149                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2
00150                 motor.setSpeed (NSpeed,-NSpeed);
00151                 if (theta < (PI/2.0))  {
00152                     etatmvt = 6;
00153                 }
00154                 break;
00155             case 6 :
00156                 // On avance de 50cm (coordonnés X = 0, Y = 0)
00157                 motor.setSpeed (NSpeed,NSpeed);
00158                 if (y > 0)  {
00159                     etatmvt = 7;
00160                 }
00161                 break;
00162             case 7 :
00163                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0
00164                 motor.setSpeed (NSpeed,-NSpeed);
00165                 if (theta < 0) {
00166                     etatmvt = 0;
00167                 }
00168                 break;
00169         }
00170 
00171         pc.printf ("\n\n");
00172         led1 = !led1;
00173         led2 = !led2;
00174         wait (0.2);
00175     }
00176 }