AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_us2

Test

Dependencies:   mbed DRV8825

captUS.cpp@26:4670968fbc63, 2020-10-28 (annotated)

Committer:
g0dd4
Date:
Wed Oct 28 23:05:19 2020 +0000
Revision:
26:4670968fbc63
Parent:
25:216e62f05f82 
Ajout OFFSET;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Nanaud 22:f891c2bce091 1 /* #include */
Nanaud 0:dc036b67c87c 2 #include "pins.h"
Nanaud 22:f891c2bce091 3
Nanaud 24:be2b2be6907b 4 /* #define */
g0dd4 26:4670968fbc63 5 #define DISTLIM 800
g0dd4 26:4670968fbc63 6 #define OFFSET 200
g0dd4 25:216e62f05f82 7 #define MAXMOY 1
Nanaud 24:be2b2be6907b 8
Nanaud 22:f891c2bce091 9 /* Variables globales */
Nanaud 22:f891c2bce091 10 Timer TimUS;
Nanaud 22:f891c2bce091 11 Ticker TickUS_actu;
Nanaud 0:dc036b67c87c 12
Nanaud 24:be2b2be6907b 13 //const float DISTLIM = 600;
Nanaud 22:f891c2bce091 14
Nanaud 24:be2b2be6907b 15 unsigned int us_high[6]= {0};
Nanaud 24:be2b2be6907b 16 unsigned int us_low[6]= {0};
g0dd4 25:216e62f05f82 17 bool usVerifOrder[2][6] = {0};
g0dd4 25:216e62f05f82 18
Nanaud 21:e5f0f5abb5ae 19
Nanaud 24:be2b2be6907b 20 unsigned int distt[6];
Nanaud 20:7d206773f39e 21 bool rebooted = 0;
Nanaud 20:7d206773f39e 22 bool wtt = 0;
Nanaud 0:dc036b67c87c 23
Nanaud 24:be2b2be6907b 24 int indix = 0;
Nanaud 24:be2b2be6907b 25 unsigned int total[6] = {0};
Nanaud 24:be2b2be6907b 26 unsigned int ttt[6] = {0};
Nanaud 24:be2b2be6907b 27
Nanaud 24:be2b2be6907b 28 void calculTotal()
Nanaud 24:be2b2be6907b 29 {
Nanaud 24:be2b2be6907b 30 if(indix < MAXMOY) {
Nanaud 24:be2b2be6907b 31 for(int j=0; j<6 ; j++) {
Nanaud 24:be2b2be6907b 32 total[j]+=distt[j];
Nanaud 24:be2b2be6907b 33 }
Nanaud 24:be2b2be6907b 34 indix++;
Nanaud 24:be2b2be6907b 35 } else {
Nanaud 24:be2b2be6907b 36 for(int j=0; j<6 ; j++) {
Nanaud 24:be2b2be6907b 37 ttt[j]=total[j]/MAXMOY;
Nanaud 24:be2b2be6907b 38 total[j]=0;
Nanaud 24:be2b2be6907b 39 }
Nanaud 24:be2b2be6907b 40
Nanaud 24:be2b2be6907b 41 /*
g0dd4 25:216e62f05f82 42 bt.printf("ttt[1] = %d\n\r", ttt[0]);
g0dd4 25:216e62f05f82 43 bt.printf("ttt[2] = %d\n\r", ttt[1]);
g0dd4 25:216e62f05f82 44 bt.printf("ttt[3] = %d\n\r", ttt[2]);
g0dd4 25:216e62f05f82 45 bt.printf("ttt[4] = %d\n\r", ttt[3]);
g0dd4 25:216e62f05f82 46 bt.printf("ttt[5] = %d\n\r", ttt[4]);
g0dd4 25:216e62f05f82 47 bt.printf("ttt[6] = %d\n\r", ttt[5]);
g0dd4 25:216e62f05f82 48 bt.printf("\n\r");
Nanaud 24:be2b2be6907b 49 */
Nanaud 24:be2b2be6907b 50
Nanaud 24:be2b2be6907b 51 indix=0;
Nanaud 24:be2b2be6907b 52 }
Nanaud 24:be2b2be6907b 53 }
Nanaud 20:7d206773f39e 54
Nanaud 21:e5f0f5abb5ae 55 /*
Nanaud 20:7d206773f39e 56 void captUS_init()
Nanaud 20:7d206773f39e 57 {
Nanaud 21:e5f0f5abb5ae 58 ::distance = new double(6); //équivalent au malloc()
Nanaud 20:7d206773f39e 59 tps.reset();
Nanaud 20:7d206773f39e 60 tps.start();
Nanaud 20:7d206773f39e 61 }
Nanaud 21:e5f0f5abb5ae 62 */
Nanaud 20:7d206773f39e 63
Nanaud 20:7d206773f39e 64 void captUS_trig()
Nanaud 20:7d206773f39e 65 {
Nanaud 20:7d206773f39e 66 convertToDistance();
Nanaud 20:7d206773f39e 67
Nanaud 24:be2b2be6907b 68 calculTotal();
Nanaud 24:be2b2be6907b 69
Nanaud 20:7d206773f39e 70 if((objRecule[indice]==0) && (fnc == 2)) {
Nanaud 22:f891c2bce091 71 //if ((distt[5] >= DISTLIM) && (distt[0] >= DISTLIM) && (distt[1] >= DISTLIM)) {
Nanaud 24:be2b2be6907b 72 //if (((distt[5] >= DISTLIM) && (distt[0] >= DISTLIM) && (distt[1] >= DISTLIM)) /*|| ((distt[5] < 0) && (distt[0] < 0) && (distt[1] < 0))*/) {
g0dd4 26:4670968fbc63 73 if ((ttt[5] >= DISTLIM-OFFSET) && (ttt[0] >= DISTLIM) && (ttt[1] >= DISTLIM-OFFSET)) {
Nanaud 20:7d206773f39e 74 wtt = 1;
Nanaud 21:e5f0f5abb5ae 75 }
Nanaud 1:2fe8c402ee79 76
Nanaud 21:e5f0f5abb5ae 77 else {
Nanaud 20:7d206773f39e 78 mot_dis();
Nanaud 20:7d206773f39e 79 rebooted = 1;
Nanaud 20:7d206773f39e 80 wtt=0;
Nanaud 20:7d206773f39e 81 }
Nanaud 20:7d206773f39e 82 }
Nanaud 20:7d206773f39e 83
Nanaud 20:7d206773f39e 84 else if((objRecule[indice]==1) && (fnc == 2)) {
Nanaud 22:f891c2bce091 85 //if ((distt[2] >= DISTLIM) && (distt[3] >= DISTLIM) && (distt[4] >= DISTLIM)) {
Nanaud 24:be2b2be6907b 86 //if (((distt[2] >= DISTLIM) && (distt[3] >= DISTLIM) && (distt[4] >= DISTLIM)) /*|| ((distt[2] < 0) && (distt[3] < 0) && (distt[4] < 0))*/) {
g0dd4 26:4670968fbc63 87 if ((ttt[2] >= DISTLIM-OFFSET) && (ttt[3] >= DISTLIM) && (ttt[4] >= DISTLIM-OFFSET)) {
Nanaud 20:7d206773f39e 88 wtt=1;
Nanaud 21:e5f0f5abb5ae 89 }
Nanaud 20:7d206773f39e 90
Nanaud 21:e5f0f5abb5ae 91 else {
Nanaud 20:7d206773f39e 92 mot_dis();
Nanaud 20:7d206773f39e 93 rebooted = 1;
Nanaud 20:7d206773f39e 94 wtt=0;
Nanaud 20:7d206773f39e 95 }
Nanaud 20:7d206773f39e 96 }
Nanaud 20:7d206773f39e 97
Nanaud 21:e5f0f5abb5ae 98 /*
Nanaud 21:e5f0f5abb5ae 99 pc.printf("US1 = %5.0lf uS\n\r", us_out[0]);
Nanaud 21:e5f0f5abb5ae 100 pc.printf("US2 = %5.0lf uS\n\r", us_out[1]);
Nanaud 21:e5f0f5abb5ae 101 //pc.printf("US3 = %5.0lf uS\n\r", us_out[2]);
Nanaud 21:e5f0f5abb5ae 102 //pc.printf("US4 = %5.0lf uS\n\r", us_out[3]);
Nanaud 21:e5f0f5abb5ae 103 //pc.printf("US5 = %5.0lf uS\n\r", us_out[4]);
Nanaud 21:e5f0f5abb5ae 104 pc.printf("US6 = %5.0lf uS\n\r", us_out[5]);
Nanaud 21:e5f0f5abb5ae 105 pc.printf("\n\r");
Nanaud 21:e5f0f5abb5ae 106 */
Nanaud 21:e5f0f5abb5ae 107
Nanaud 21:e5f0f5abb5ae 108 /*
Nanaud 21:e5f0f5abb5ae 109 pc.printf("Dist1 = %5.0lf mm\n\r", ::distance[0]);
Nanaud 21:e5f0f5abb5ae 110 pc.printf("Dist2 = %5.0lf mm\n\r", ::distance[1]);
Nanaud 21:e5f0f5abb5ae 111 pc.printf("Dist3 = %5.0lf mm\n\r", ::distance[2]);
Nanaud 21:e5f0f5abb5ae 112 pc.printf("Dist4 = %5.0lf mm\n\r", ::distance[3]);
Nanaud 21:e5f0f5abb5ae 113 pc.printf("Dist5 = %5.0lf mm\n\r", ::distance[4]);
Nanaud 21:e5f0f5abb5ae 114 pc.printf("Dist6 = %5.0lf mm\n\r", ::distance[5]);
Nanaud 21:e5f0f5abb5ae 115 pc.printf("\n\r");
Nanaud 21:e5f0f5abb5ae 116 */
Nanaud 24:be2b2be6907b 117
Nanaud 24:be2b2be6907b 118 /*
Nanaud 24:be2b2be6907b 119 pc.printf("ttt[1] = %d\n\r", ttt[0]);
Nanaud 24:be2b2be6907b 120 pc.printf("ttt[2] = %d\n\r", ttt[1]);
Nanaud 24:be2b2be6907b 121 pc.printf("ttt[3] = %d\n\r", ttt[2]);
Nanaud 24:be2b2be6907b 122 pc.printf("ttt[4] = %d\n\r", ttt[3]);
Nanaud 24:be2b2be6907b 123 pc.printf("ttt[5] = %d\n\r", ttt[4]);
Nanaud 24:be2b2be6907b 124 pc.printf("ttt[6] = %d\n\r", ttt[5]);
Nanaud 24:be2b2be6907b 125 pc.printf("\n\r");
Nanaud 24:be2b2be6907b 126 */
Nanaud 21:e5f0f5abb5ae 127
Nanaud 24:be2b2be6907b 128 //calculTotal();
Nanaud 23:a74135a0271d 129
Nanaud 22:f891c2bce091 130 TimUS.reset();
Nanaud 0:dc036b67c87c 131 trigger=1;
Nanaud 0:dc036b67c87c 132 wait(0.00002);
Nanaud 0:dc036b67c87c 133 trigger=0;
Nanaud 0:dc036b67c87c 134 }
Nanaud 0:dc036b67c87c 135
Nanaud 20:7d206773f39e 136 void echoRise1()
Nanaud 20:7d206773f39e 137 {
g0dd4 25:216e62f05f82 138 if(usVerifOrder[1][0] == false) {
Nanaud 22:f891c2bce091 139 us_high[0]=TimUS.read_us();
g0dd4 25:216e62f05f82 140 usVerifOrder[0][0] = true;
Nanaud 21:e5f0f5abb5ae 141 }
Nanaud 20:7d206773f39e 142 }
Nanaud 21:e5f0f5abb5ae 143
Nanaud 20:7d206773f39e 144 void echoFall1()
Nanaud 20:7d206773f39e 145 {
g0dd4 25:216e62f05f82 146 if(usVerifOrder[0][0] == true) {
Nanaud 22:f891c2bce091 147 us_low[0]=TimUS.read_us();
g0dd4 25:216e62f05f82 148 usVerifOrder[1][0] = true;
Nanaud 21:e5f0f5abb5ae 149 }
Nanaud 20:7d206773f39e 150 }
Nanaud 20:7d206773f39e 151
Nanaud 20:7d206773f39e 152 void echoRise2()
Nanaud 20:7d206773f39e 153 {
g0dd4 25:216e62f05f82 154 if(usVerifOrder[1][1] == false) {
Nanaud 22:f891c2bce091 155 us_high[1]=TimUS.read_us();
g0dd4 25:216e62f05f82 156 usVerifOrder[0][1] = true;
Nanaud 21:e5f0f5abb5ae 157 }
Nanaud 20:7d206773f39e 158 }
Nanaud 21:e5f0f5abb5ae 159
Nanaud 20:7d206773f39e 160 void echoFall2()
Nanaud 20:7d206773f39e 161 {
g0dd4 25:216e62f05f82 162 if(usVerifOrder[0][1] == true) {
Nanaud 22:f891c2bce091 163 us_low[1]=TimUS.read_us();
g0dd4 25:216e62f05f82 164 usVerifOrder[1][1] = true;
Nanaud 21:e5f0f5abb5ae 165 }
Nanaud 20:7d206773f39e 166 }
Nanaud 20:7d206773f39e 167
Nanaud 20:7d206773f39e 168 void echoRise3()
Nanaud 20:7d206773f39e 169 {
g0dd4 25:216e62f05f82 170 if(usVerifOrder[1][2] == false) {
Nanaud 22:f891c2bce091 171 us_high[2]=TimUS.read_us();
g0dd4 25:216e62f05f82 172 usVerifOrder[0][2] = true;
Nanaud 21:e5f0f5abb5ae 173 }
Nanaud 20:7d206773f39e 174 }
Nanaud 21:e5f0f5abb5ae 175
Nanaud 20:7d206773f39e 176 void echoFall3()
Nanaud 20:7d206773f39e 177 {
g0dd4 25:216e62f05f82 178 if(usVerifOrder[0][2] == true) {
Nanaud 22:f891c2bce091 179 us_low[2]=TimUS.read_us();
g0dd4 25:216e62f05f82 180 usVerifOrder[1][2] = true;
Nanaud 21:e5f0f5abb5ae 181 }
Nanaud 20:7d206773f39e 182 }
Nanaud 0:dc036b67c87c 183
Nanaud 20:7d206773f39e 184 void echoRise4()
Nanaud 20:7d206773f39e 185 {
g0dd4 25:216e62f05f82 186 if(usVerifOrder[1][3] == false) {
Nanaud 22:f891c2bce091 187 us_high[3]=TimUS.read_us();
g0dd4 25:216e62f05f82 188 usVerifOrder[0][3] = true;
Nanaud 21:e5f0f5abb5ae 189 }
Nanaud 20:7d206773f39e 190 }
Nanaud 21:e5f0f5abb5ae 191
Nanaud 20:7d206773f39e 192 void echoFall4()
Nanaud 20:7d206773f39e 193 {
g0dd4 25:216e62f05f82 194 if(usVerifOrder[0][3] == true) {
Nanaud 22:f891c2bce091 195 us_low[3]=TimUS.read_us();
g0dd4 25:216e62f05f82 196 usVerifOrder[1][3] = true;
Nanaud 21:e5f0f5abb5ae 197 }
Nanaud 20:7d206773f39e 198 }
Nanaud 0:dc036b67c87c 199
Nanaud 20:7d206773f39e 200 void echoRise5()
g0dd4 25:216e62f05f82 201 {
g0dd4 25:216e62f05f82 202 if(usVerifOrder[1][4] == false) {
Nanaud 22:f891c2bce091 203 us_high[4]=TimUS.read_us();
g0dd4 25:216e62f05f82 204 usVerifOrder[0][4] = true;
Nanaud 21:e5f0f5abb5ae 205 }
Nanaud 20:7d206773f39e 206 }
Nanaud 21:e5f0f5abb5ae 207
Nanaud 20:7d206773f39e 208 void echoFall5()
Nanaud 20:7d206773f39e 209 {
g0dd4 25:216e62f05f82 210 if(usVerifOrder[0][4] == true) {
Nanaud 22:f891c2bce091 211 us_low[4]=TimUS.read_us();
g0dd4 25:216e62f05f82 212 usVerifOrder[1][4] = true;
Nanaud 21:e5f0f5abb5ae 213 }
Nanaud 20:7d206773f39e 214 }
Nanaud 20:7d206773f39e 215
Nanaud 20:7d206773f39e 216 void echoRise6()
Nanaud 20:7d206773f39e 217 {
g0dd4 25:216e62f05f82 218 if(usVerifOrder[1][5] == false) {
Nanaud 22:f891c2bce091 219 us_high[5]=TimUS.read_us();
g0dd4 25:216e62f05f82 220 usVerifOrder[0][5] = true;
Nanaud 21:e5f0f5abb5ae 221 }
Nanaud 20:7d206773f39e 222 }
Nanaud 21:e5f0f5abb5ae 223
Nanaud 20:7d206773f39e 224 void echoFall6()
Nanaud 20:7d206773f39e 225 {
g0dd4 25:216e62f05f82 226 if(usVerifOrder[0][5] == true) {
Nanaud 22:f891c2bce091 227 us_low[5]=TimUS.read_us();
g0dd4 25:216e62f05f82 228 usVerifOrder[1][5] = true;
Nanaud 21:e5f0f5abb5ae 229 }
Nanaud 20:7d206773f39e 230 }
Nanaud 20:7d206773f39e 231
Nanaud 0:dc036b67c87c 232
Nanaud 21:e5f0f5abb5ae 233 void convertToDistance()
Nanaud 20:7d206773f39e 234 {
Nanaud 20:7d206773f39e 235 /**************************************
Nanaud 20:7d206773f39e 236 * Nous convertisons grâce au valeur *
Nanaud 20:7d206773f39e 237 * qui sont retournées par echoRiseX *
Nanaud 20:7d206773f39e 238 * et echoFallx *
Nanaud 20:7d206773f39e 239 **************************************/
Nanaud 20:7d206773f39e 240
Nanaud 21:e5f0f5abb5ae 241 for(int i = 0; i<6; i++) {
g0dd4 25:216e62f05f82 242 if(usVerifOrder[0][i] == true && usVerifOrder[1][i] == true)
g0dd4 25:216e62f05f82 243 distt[i] = (int) (((us_low[i] - us_high[i])*340)/1000); //conversion en distance(mm)
g0dd4 25:216e62f05f82 244 else
g0dd4 25:216e62f05f82 245 distt[i] = LARGEUR_TAB;
Nanaud 20:7d206773f39e 246 }
Nanaud 20:7d206773f39e 247
Nanaud 20:7d206773f39e 248 /****************************************
Nanaud 20:7d206773f39e 249 * nous retournons l'adresse du tableau *
Nanaud 20:7d206773f39e 250 ****************************************/
Nanaud 21:e5f0f5abb5ae 251 //return ::distance;
Nanaud 20:7d206773f39e 252 }