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 
00011 #define PI  3.1415926535898
00012 
00013 
00014 #define VMOY    300
00015 #define VMOY2   120
00016 #define k       0.84
00017 #define ouvert  0.8
00018 #define fermer  1.0
00019 
00020 Serial      pc          (PA_2, PA_3, 921600);
00021 PID         motor       (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);
00022 
00023 CMPS03      boussole    (PC_4);
00024 
00025 CNY70       ligneD      (PC_3);
00026 CNY70       ligneG      (PC_2);
00027 CNY70       exterior    (PA_7);
00028 
00029 //GP2A        sd1         (PB_0, 5, 80, 10);
00030 AnalogIn  sd1(PB_0);
00031 
00032 RC_Servo    ballon      (PB_10, 1);
00033 RC_Servo    verrou      (PA_15, 1);
00034 
00035 VMA306      UltraSon    (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2);
00036 
00037 PIXY        Pixy        (PA_0, PA_1, 115200);
00038 
00039 DigitalIn   bp          (PC_13);
00040 
00041 
00042 DigitalOut  disquette   (PA_12);
00043 
00044 InterruptIn button1(USER_BUTTON);
00045 
00046 Timer t1,t2;
00047 int gietat=0;
00048 double gCap=0;
00049 double gBoussole=0;
00050 double distance_petit=0;
00051 double erreur_cap;
00052 long                taille_balle;
00053 void start()
00054 {
00055     gietat=1;
00056     gCap=boussole.getBearing();
00057     t2.reset();
00058 }
00059 
00060 int main()
00061 {
00062 
00063     int                 position_balle=0;
00064     int                 nbCC, nbNM;
00065     T_pixyCCBloc        CCBuf;
00066     T_pixyNMBloc        NMBuf;
00067     disquette = 0;
00068     motor.resetPosition();
00069     verrou.write(ouvert);
00070     button1.fall(&start);
00071     t1.start();
00072     t2.start();
00073 
00074     while(1) {
00075         gBoussole=boussole.getBearing();
00076         distance_petit=(10.00/(sd1.read()*3.30-0.50))-0.42;
00077         if(distance_petit>20.0)distance_petit=20;
00078         if(distance_petit<3.5)distance_petit=3.5;
00079         erreur_cap=gCap-gBoussole;
00080         //pc.printf("etat:%d \t erreur:%0.2f \t distance:%0.2fn\n\r",gietat,abs(erreur_cap),distance_petit);
00081         switch(gietat) {
00082             case 0:
00083                 break;
00084             case 1:
00085                 if (Pixy.checkNewImage()) {
00086                         gietat=2;
00087                     }
00088                 if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) {
00089                     gietat=6;
00090                     t1.reset();
00091                 }
00092                 if(t2.read()>=85) gietat=8;
00093                 break;
00094             case 2:
00095                 if( distance_petit<=4.50 && distance_petit>=3.8) {
00096                     gietat=30;
00097                     t1.reset();
00098                 }
00099                 if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) {
00100                     gietat=6;
00101                     t1.reset();
00102                 }
00103                 if(t2.read()>=85) gietat=8;
00104                 break;
00105 
00106             case 3:
00107                 if(abs(erreur_cap)<=7.0) {
00108                     gietat=4;
00109                     verrou.write(ouvert);
00110                 }
00111                 if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) {
00112                     gietat=6;
00113                     t1.reset();
00114                 }
00115                 if(t2.read()>=85) gietat=8;
00116                 break;
00117             case 4:
00118                 if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) {
00119                     gietat=5;
00120                 }
00121                 if(t2.read()>=85) gietat=8;
00122                 break;
00123             case 5:
00124                 t1.reset();
00125                 gietat=6;
00126                 break;
00127             case 6:
00128                 if(t1.read()>=2) {
00129                     gietat=7;
00130                     t1.reset();
00131                 }
00132                 if(t2.read()>=85) gietat=8;
00133                 break;
00134             case 7:
00135                 if (t1.read()>1.75) {
00136                     gietat=70;
00137                     t1.reset();
00138                 }
00139                 if(t2.read()>=85) gietat=8;
00140                 break;
00141             case 70:
00142             if (Pixy.checkNewImage()) {
00143                         gietat=2;
00144                     }
00145                 if (t1.read()>0.75) {
00146                     gietat=71;
00147                     t1.reset();
00148                 }
00149                 
00150                 if(t2.read()>=85) gietat=8;
00151                 break;
00152             case 71: 
00153                 if (Pixy.checkNewImage()) {
00154                         gietat=2;
00155                     }
00156                 if (t1.read()>0.75) {
00157                     gietat=71;
00158                     t1.reset();
00159                 }
00160                 if(t2.read()>=85) gietat=8;
00161                 break;
00162             case 30:
00163                 if(t1.read()>0.75) {
00164                     gietat=31;
00165                     t1.reset();
00166                 }
00167                 if(t2.read()>=85) gietat=8;
00168                 break;
00169             case 31:
00170                 if(t1.read()>0.2) {
00171                     gietat=3;
00172                     t1.reset();
00173                 }
00174                 if(t2.read()>=85) gietat=8;
00175                 break;
00176         }
00177         switch(gietat) {
00178             case 0:
00179                 motor.setSpeed(0,0);
00180                 break;
00181             case 1:
00182                 motor.setSpeed(-200,200);
00183                 break;
00184             case 2:
00185                 Pixy.detectedObject (&nbNM, &nbCC);
00186                 if (nbNM > 0) {
00187                     NMBuf = Pixy.getNMBloc ();
00188                     nbNM--;
00189                     //pc.printf ("\rNM %4x : x=%5d, y=%5d - w=%5d, h=%5d\n", NMBuf.signature, NMBuf.x, NMBuf.y, NMBuf.width, NMBuf.height);
00190                     position_balle=NMBuf.x-160;
00191                     //pc.printf("position_balle:%d \n\r",position_balle);
00192                     motor.setSpeed((int)((VMOY+(k*position_balle))),(int)((VMOY-(k*position_balle))));
00193                 }
00194                 break;
00195             case 3:
00196                 motor.setSpeed(-150,150);
00197                 verrou.write(fermer);
00198                 break;
00199             case 30:
00200                 motor.setSpeed(25,25);
00201                 break;
00202             case 31:
00203                 motor.setSpeed(25,25);
00204                 verrou.write(fermer);
00205                 break;
00206             case 4:
00207                 motor.setSpeed(600,600);
00208                 break;
00209             case 5:
00210                 motor.setSpeed(0,0);
00211                 break;
00212             case 6:
00213                 motor.setSpeed(-150,-150);
00214                 break;
00215             case 7:
00216                 motor.setSpeed(-150,150);
00217                 break;
00218             case 8:
00219                 motor.setSpeed(0,0);
00220                 break;
00221             case 70:
00222                 motor.setSpeed(-150,150);
00223                 break;
00224             case 71:
00225                 motor.setSpeed(150,-150);
00226                 break;
00227 
00228         }
00229     }
00230 }