captUS.cpp

00001 /* #include */
00002 #include "pins.h"
00003 
00004 /* #define */
00005 #define DISTLIM 800
00006 #define OFFSET 200
00007 #define MAXMOY 1
00008 
00009 /* Variables globales */
00010 Timer TimUS;
00011 Ticker TickUS_actu;
00012 
00013 //const float DISTLIM = 600;
00014 
00015 unsigned int us_high[6]= {0};
00016 unsigned int us_low[6]= {0};
00017 bool usVerifOrder[2][6] = {0};
00018 
00019 
00020 unsigned int distt[6];
00021 bool rebooted = 0;
00022 bool wtt = 0;
00023 
00024 int indix = 0;
00025 unsigned int total[6] = {0};
00026 unsigned int ttt[6] = {0};
00027 
00028 void calculTotal()
00029 {
00030     if(indix < MAXMOY) {
00031         for(int j=0; j<6 ; j++) {
00032             total[j]+=distt[j];
00033         }
00034         indix++;
00035     } else {
00036         for(int j=0; j<6 ; j++) {
00037             ttt[j]=total[j]/MAXMOY;
00038             total[j]=0;
00039         }
00040 
00041         /*
00042         bt.printf("ttt[1] = %d\n\r", ttt[0]);
00043         bt.printf("ttt[2] = %d\n\r", ttt[1]);
00044         bt.printf("ttt[3] = %d\n\r", ttt[2]);
00045         bt.printf("ttt[4] = %d\n\r", ttt[3]);
00046         bt.printf("ttt[5] = %d\n\r", ttt[4]);
00047         bt.printf("ttt[6] = %d\n\r", ttt[5]);
00048         bt.printf("\n\r");
00049         */
00050 
00051         indix=0;
00052     }
00053 }
00054 
00055 /*
00056 void captUS_init()
00057 {
00058     ::distance = new double(6); //équivalent au malloc()
00059     tps.reset();
00060     tps.start();
00061 }
00062 */
00063 
00064 void captUS_trig()
00065 {
00066     convertToDistance();
00067 
00068     calculTotal();
00069 
00070     if((objRecule[indice]==0) && (fnc == 2)) {
00071         //if ((distt[5] >= DISTLIM) && (distt[0] >= DISTLIM)  && (distt[1] >= DISTLIM)) {
00072         //if (((distt[5] >= DISTLIM) && (distt[0] >= DISTLIM)  && (distt[1] >= DISTLIM)) /*|| ((distt[5] < 0) && (distt[0] < 0)  && (distt[1] < 0))*/) {
00073         if ((ttt[5] >= DISTLIM-OFFSET) && (ttt[0] >= DISTLIM)  && (ttt[1] >= DISTLIM-OFFSET)) {
00074             wtt = 1;
00075         }
00076 
00077         else {
00078             mot_dis();
00079             rebooted = 1;
00080             wtt=0;
00081         }
00082     }
00083 
00084     else if((objRecule[indice]==1) && (fnc == 2)) {
00085         //if ((distt[2] >= DISTLIM) && (distt[3] >= DISTLIM) && (distt[4] >= DISTLIM)) {
00086         //if (((distt[2] >= DISTLIM) && (distt[3] >= DISTLIM)  && (distt[4] >= DISTLIM)) /*|| ((distt[2] < 0) && (distt[3] < 0)  && (distt[4] < 0))*/) {
00087         if ((ttt[2] >= DISTLIM-OFFSET) && (ttt[3] >= DISTLIM)  && (ttt[4] >= DISTLIM-OFFSET)) {
00088             wtt=1;
00089         }
00090 
00091         else {
00092             mot_dis();
00093             rebooted = 1;
00094             wtt=0;
00095         }
00096     }
00097 
00098     /*
00099     pc.printf("US1 = %5.0lf uS\n\r", us_out[0]);
00100     pc.printf("US2 = %5.0lf uS\n\r", us_out[1]);
00101     //pc.printf("US3 = %5.0lf uS\n\r", us_out[2]);
00102     //pc.printf("US4 = %5.0lf uS\n\r", us_out[3]);
00103     //pc.printf("US5 = %5.0lf uS\n\r", us_out[4]);
00104     pc.printf("US6 = %5.0lf uS\n\r", us_out[5]);
00105     pc.printf("\n\r");
00106     */
00107 
00108     /*
00109     pc.printf("Dist1 = %5.0lf mm\n\r", ::distance[0]);
00110     pc.printf("Dist2 = %5.0lf mm\n\r", ::distance[1]);
00111     pc.printf("Dist3 = %5.0lf mm\n\r", ::distance[2]);
00112     pc.printf("Dist4 = %5.0lf mm\n\r", ::distance[3]);
00113     pc.printf("Dist5 = %5.0lf mm\n\r", ::distance[4]);
00114     pc.printf("Dist6 = %5.0lf mm\n\r", ::distance[5]);
00115     pc.printf("\n\r");
00116     */
00117     
00118     /*
00119     pc.printf("ttt[1] = %d\n\r", ttt[0]);
00120     pc.printf("ttt[2] = %d\n\r", ttt[1]);
00121     pc.printf("ttt[3] = %d\n\r", ttt[2]);
00122     pc.printf("ttt[4] = %d\n\r", ttt[3]);
00123     pc.printf("ttt[5] = %d\n\r", ttt[4]);
00124     pc.printf("ttt[6] = %d\n\r", ttt[5]);
00125     pc.printf("\n\r");
00126     */
00127 
00128     //calculTotal();
00129 
00130     TimUS.reset();
00131     trigger=1;
00132     wait(0.00002);
00133     trigger=0;
00134 }
00135 
00136 void echoRise1()
00137 {
00138     if(usVerifOrder[1][0] == false) {
00139         us_high[0]=TimUS.read_us();
00140         usVerifOrder[0][0] = true; 
00141     }
00142 }
00143 
00144 void echoFall1()
00145 {
00146     if(usVerifOrder[0][0] == true) {
00147         us_low[0]=TimUS.read_us();
00148         usVerifOrder[1][0] = true;
00149     }
00150 }
00151 
00152 void echoRise2()
00153 {
00154     if(usVerifOrder[1][1] == false) {
00155         us_high[1]=TimUS.read_us();
00156         usVerifOrder[0][1] = true;
00157     }
00158 }
00159 
00160 void echoFall2()
00161 {
00162     if(usVerifOrder[0][1] == true) {
00163         us_low[1]=TimUS.read_us();
00164         usVerifOrder[1][1] = true;
00165     }
00166 }
00167 
00168 void echoRise3()
00169 {
00170     if(usVerifOrder[1][2] == false) {
00171         us_high[2]=TimUS.read_us();
00172         usVerifOrder[0][2] = true;
00173     }
00174 }
00175 
00176 void echoFall3()
00177 {
00178     if(usVerifOrder[0][2] == true) {
00179         us_low[2]=TimUS.read_us();
00180         usVerifOrder[1][2] = true;
00181     }
00182 }
00183 
00184 void echoRise4()
00185 {
00186     if(usVerifOrder[1][3] == false) {
00187         us_high[3]=TimUS.read_us();
00188         usVerifOrder[0][3] = true;
00189     }
00190 }
00191 
00192 void echoFall4()
00193 {
00194     if(usVerifOrder[0][3] == true) {
00195         us_low[3]=TimUS.read_us();
00196         usVerifOrder[1][3] = true;
00197     }
00198 }
00199 
00200 void echoRise5()
00201 {    
00202     if(usVerifOrder[1][4] == false) {
00203         us_high[4]=TimUS.read_us();
00204         usVerifOrder[0][4] = true;
00205     }
00206 }
00207 
00208 void echoFall5()
00209 {
00210     if(usVerifOrder[0][4] == true) {
00211         us_low[4]=TimUS.read_us();
00212         usVerifOrder[1][4] = true;
00213     }
00214 }
00215 
00216 void echoRise6()
00217 {
00218     if(usVerifOrder[1][5] == false) {
00219         us_high[5]=TimUS.read_us();
00220         usVerifOrder[0][5] = true;
00221     }
00222 }
00223 
00224 void echoFall6()
00225 {
00226     if(usVerifOrder[0][5] == true) {
00227         us_low[5]=TimUS.read_us();
00228         usVerifOrder[1][5] = true;
00229     }
00230 }
00231 
00232 
00233 void convertToDistance()
00234 {
00235     /**************************************
00236      * Nous convertisons grâce au valeur  *
00237      * qui sont retournées par echoRiseX  *
00238      * et echoFallx                       *
00239      **************************************/
00240 
00241     for(int i = 0; i<6; i++) {
00242         if(usVerifOrder[0][i] == true && usVerifOrder[1][i] == true)
00243             distt[i] = (int) (((us_low[i] - us_high[i])*340)/1000); //conversion en distance(mm)
00244         else
00245             distt[i] = LARGEUR_TAB;
00246     }
00247 
00248     /****************************************
00249      * nous retournons l'adresse du tableau *
00250      ****************************************/
00251     //return ::distance;
00252 }