AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed DRV8825

captUS.cpp@24:be2b2be6907b, 2020-10-28 (annotated)

Committer:
Nanaud
Date:
Wed Oct 28 20:08:39 2020 +0000
Revision:
24:be2b2be6907b
Parent:
23:a74135a0271d
Child:
25:869b1c1f51a7
Homologue

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