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@23:a74135a0271d, 2020-10-28 (annotated)

Committer:
Nanaud
Date:
Wed Oct 28 16:27:05 2020 +0000
Revision:
23:a74135a0271d
Parent:
22:f891c2bce091
Child:
24:be2b2be6907b
avant => double to int

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