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@22:f891c2bce091, 2020-10-27 (annotated)

Committer:
Nanaud
Date:
Tue Oct 27 17:27:33 2020 +0000
Revision:
22:f891c2bce091
Parent:
21:e5f0f5abb5ae
Child:
23:a74135a0271d
Homologation J-2

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 22:f891c2bce091 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 22:f891c2bce091 80 TimUS.reset();
Nanaud 0:dc036b67c87c 81 trigger=1;
Nanaud 0:dc036b67c87c 82 wait(0.00002);
Nanaud 0:dc036b67c87c 83 trigger=0;
Nanaud 0:dc036b67c87c 84 }
Nanaud 0:dc036b67c87c 85
Nanaud 20:7d206773f39e 86 void echoRise1()
Nanaud 20:7d206773f39e 87 {
Nanaud 21:e5f0f5abb5ae 88 if(us_verif[0] == 0) {
Nanaud 22:f891c2bce091 89 us_high[0]=TimUS.read_us();
Nanaud 21:e5f0f5abb5ae 90 us_verif[0] = 1;
Nanaud 21:e5f0f5abb5ae 91 }
Nanaud 20:7d206773f39e 92 }
Nanaud 21:e5f0f5abb5ae 93
Nanaud 20:7d206773f39e 94 void echoFall1()
Nanaud 20:7d206773f39e 95 {
Nanaud 21:e5f0f5abb5ae 96 if(us_verif[0] == 1) {
Nanaud 22:f891c2bce091 97 us_low[0]=TimUS.read_us();
Nanaud 21:e5f0f5abb5ae 98 us_diff[0]=us_low[0]-us_high[0];
Nanaud 21:e5f0f5abb5ae 99 us_verif[0] = 0;
Nanaud 21:e5f0f5abb5ae 100 }
Nanaud 20:7d206773f39e 101 }
Nanaud 20:7d206773f39e 102
Nanaud 20:7d206773f39e 103 void echoRise2()
Nanaud 20:7d206773f39e 104 {
Nanaud 21:e5f0f5abb5ae 105 if(us_verif[1] == 0) {
Nanaud 22:f891c2bce091 106 us_high[1]=TimUS.read_us();
Nanaud 21:e5f0f5abb5ae 107 us_verif[1] = 1;
Nanaud 21:e5f0f5abb5ae 108 }
Nanaud 20:7d206773f39e 109 }
Nanaud 21:e5f0f5abb5ae 110
Nanaud 20:7d206773f39e 111 void echoFall2()
Nanaud 20:7d206773f39e 112 {
Nanaud 21:e5f0f5abb5ae 113 if(us_verif[1] == 1) {
Nanaud 22:f891c2bce091 114 us_low[1]=TimUS.read_us();
Nanaud 21:e5f0f5abb5ae 115 us_diff[1]=us_low[1]-us_high[1];
Nanaud 21:e5f0f5abb5ae 116 us_verif[1] = 0;
Nanaud 21:e5f0f5abb5ae 117 }
Nanaud 20:7d206773f39e 118 }
Nanaud 20:7d206773f39e 119
Nanaud 20:7d206773f39e 120 void echoRise3()
Nanaud 20:7d206773f39e 121 {
Nanaud 21:e5f0f5abb5ae 122 if(us_verif[2] == 0) {
Nanaud 22:f891c2bce091 123 us_high[2]=TimUS.read_us();
Nanaud 21:e5f0f5abb5ae 124 us_verif[2] = 1;
Nanaud 21:e5f0f5abb5ae 125 }
Nanaud 20:7d206773f39e 126 }
Nanaud 21:e5f0f5abb5ae 127
Nanaud 20:7d206773f39e 128 void echoFall3()
Nanaud 20:7d206773f39e 129 {
Nanaud 21:e5f0f5abb5ae 130 if(us_verif[2] == 1) {
Nanaud 22:f891c2bce091 131 us_low[2]=TimUS.read_us();
Nanaud 21:e5f0f5abb5ae 132 us_diff[2]=us_low[2]-us_high[2];
Nanaud 21:e5f0f5abb5ae 133 us_verif[2] = 0;
Nanaud 21:e5f0f5abb5ae 134 }
Nanaud 20:7d206773f39e 135 }
Nanaud 0:dc036b67c87c 136
Nanaud 20:7d206773f39e 137 void echoRise4()
Nanaud 20:7d206773f39e 138 {
Nanaud 21:e5f0f5abb5ae 139 if(us_verif[3] == 0) {
Nanaud 22:f891c2bce091 140 us_high[3]=TimUS.read_us();
Nanaud 21:e5f0f5abb5ae 141 us_verif[3] = 1;
Nanaud 21:e5f0f5abb5ae 142 }
Nanaud 20:7d206773f39e 143 }
Nanaud 21:e5f0f5abb5ae 144
Nanaud 20:7d206773f39e 145 void echoFall4()
Nanaud 20:7d206773f39e 146 {
Nanaud 21:e5f0f5abb5ae 147 if(us_verif[3] == 1) {
Nanaud 22:f891c2bce091 148 us_low[3]=TimUS.read_us();
Nanaud 21:e5f0f5abb5ae 149 us_diff[3]=us_low[3]-us_high[3];
Nanaud 21:e5f0f5abb5ae 150 us_verif[3] = 0;
Nanaud 21:e5f0f5abb5ae 151 }
Nanaud 20:7d206773f39e 152 }
Nanaud 0:dc036b67c87c 153
Nanaud 20:7d206773f39e 154 void echoRise5()
Nanaud 20:7d206773f39e 155 {
Nanaud 21:e5f0f5abb5ae 156 if(us_verif[4] == 0) {
Nanaud 22:f891c2bce091 157 us_high[4]=TimUS.read_us();
Nanaud 21:e5f0f5abb5ae 158 us_verif[4] = 1;
Nanaud 21:e5f0f5abb5ae 159 }
Nanaud 20:7d206773f39e 160 }
Nanaud 21:e5f0f5abb5ae 161
Nanaud 20:7d206773f39e 162 void echoFall5()
Nanaud 20:7d206773f39e 163 {
Nanaud 21:e5f0f5abb5ae 164 if(us_verif[4] == 1) {
Nanaud 22:f891c2bce091 165 us_low[4]=TimUS.read_us();
Nanaud 21:e5f0f5abb5ae 166 us_diff[4]=us_low[4]-us_high[4];
Nanaud 21:e5f0f5abb5ae 167 us_verif[4] = 0;
Nanaud 21:e5f0f5abb5ae 168 }
Nanaud 20:7d206773f39e 169 }
Nanaud 20:7d206773f39e 170
Nanaud 20:7d206773f39e 171 void echoRise6()
Nanaud 20:7d206773f39e 172 {
Nanaud 21:e5f0f5abb5ae 173 if(us_verif[5] == 0) {
Nanaud 22:f891c2bce091 174 us_high[5]=TimUS.read_us();
Nanaud 21:e5f0f5abb5ae 175 us_verif[5] = 1;
Nanaud 21:e5f0f5abb5ae 176 }
Nanaud 20:7d206773f39e 177 }
Nanaud 21:e5f0f5abb5ae 178
Nanaud 20:7d206773f39e 179 void echoFall6()
Nanaud 20:7d206773f39e 180 {
Nanaud 21:e5f0f5abb5ae 181 if(us_verif[5] == 1) {
Nanaud 22:f891c2bce091 182 us_low[5]=TimUS.read_us();
Nanaud 21:e5f0f5abb5ae 183 us_diff[5]=us_low[5]-us_high[5];
Nanaud 21:e5f0f5abb5ae 184 us_verif[5] = 0;
Nanaud 21:e5f0f5abb5ae 185 }
Nanaud 20:7d206773f39e 186 }
Nanaud 20:7d206773f39e 187
Nanaud 0:dc036b67c87c 188
Nanaud 21:e5f0f5abb5ae 189 void convertToDistance()
Nanaud 20:7d206773f39e 190 {
Nanaud 20:7d206773f39e 191 /**************************************
Nanaud 20:7d206773f39e 192 * Nous convertisons grâce au valeur *
Nanaud 20:7d206773f39e 193 * qui sont retournées par echoRiseX *
Nanaud 20:7d206773f39e 194 * et echoFallx *
Nanaud 20:7d206773f39e 195 **************************************/
Nanaud 20:7d206773f39e 196
Nanaud 21:e5f0f5abb5ae 197 for(int i = 0; i<6; i++) {
Nanaud 21:e5f0f5abb5ae 198 distt[i] = ((us_diff[i]*340)/1000); //conversion en distance(mm)
Nanaud 20:7d206773f39e 199 }
Nanaud 20:7d206773f39e 200
Nanaud 20:7d206773f39e 201 /****************************************
Nanaud 20:7d206773f39e 202 * nous retournons l'adresse du tableau *
Nanaud 20:7d206773f39e 203 ****************************************/
Nanaud 21:e5f0f5abb5ae 204 //return ::distance;
Nanaud 20:7d206773f39e 205 }