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@25:869b1c1f51a7, 2020-10-29 (annotated)

Committer:
Nanaud
Date:
Thu Oct 29 07:53:25 2020 +0000
Revision:
25:869b1c1f51a7
Parent:
24:be2b2be6907b
Child:
26:bb2b778bd351
Serie1

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 25:869b1c1f51a7 5 #define DISTLIM 600 // Distance max de détection
Nanaud 25:869b1c1f51a7 6 #define OFFSET 200
Nanaud 25:869b1c1f51a7 7 #define MAXMOY 3 // Nombre de mesure pour la moyenne
Nanaud 24:be2b2be6907b 8
Nanaud 22:f891c2bce091 9 /* Variables globales */
Nanaud 25:869b1c1f51a7 10 Timer TimUS; // Timer pour la mesure de distance entre le robot
Nanaud 25:869b1c1f51a7 11 Ticker TickUS_actu; // Actualisation valeur distance détection d'obstacle
Nanaud 22:f891c2bce091 12
Nanaud 25:869b1c1f51a7 13 unsigned int us_high[6] = {0}; // Valeurs du timer lorsqu'il y a une interruption sur front montant
Nanaud 25:869b1c1f51a7 14 unsigned int us_low[6] = {0}; // Valeurs du timer lorsqu'il y a une interruption sur front descendant
Nanaud 25:869b1c1f51a7 15 unsigned int us_diff[6] = {0}; // Différence entre ces deux temps
Nanaud 25:869b1c1f51a7 16 //bool us_verif[6] = {0}; // Permet d'enchainer dans le bon ordre : front montant puis front descendant
Nanaud 25:869b1c1f51a7 17 bool us_verif2[2][6] = {0};
Nanaud 25:869b1c1f51a7 18
Nanaud 25:869b1c1f51a7 19 unsigned int us_dist[6] = {0}; // Valeurs des distances
Nanaud 25:869b1c1f51a7 20 bool us_rbt_restart = 0; // Le robot a détecté un obstacle et s'arrête
Nanaud 25:869b1c1f51a7 21 bool us_libre = 0; // Le robot ne détecte plus d'obstacle, il est prêt à redémarrer
Nanaud 21:e5f0f5abb5ae 22
Nanaud 25:869b1c1f51a7 23 int i = 0;
Nanaud 25:869b1c1f51a7 24 unsigned int us_dist_total[6] = {0};
Nanaud 25:869b1c1f51a7 25 unsigned int us_dist_moy[6] = {0};
Nanaud 24:be2b2be6907b 26
Nanaud 25:869b1c1f51a7 27 void captUS_moyenne()
Nanaud 24:be2b2be6907b 28 {
Nanaud 25:869b1c1f51a7 29 if(i < MAXMOY) {
Nanaud 24:be2b2be6907b 30 for(int j=0; j<6 ; j++) {
Nanaud 25:869b1c1f51a7 31 us_dist_total[j] += us_dist[j];
Nanaud 24:be2b2be6907b 32 }
Nanaud 25:869b1c1f51a7 33 i++;
Nanaud 25:869b1c1f51a7 34
Nanaud 24:be2b2be6907b 35 } else {
Nanaud 24:be2b2be6907b 36 for(int j=0; j<6 ; j++) {
Nanaud 25:869b1c1f51a7 37 us_dist_moy[j]=us_dist_total[j] / MAXMOY;
Nanaud 25:869b1c1f51a7 38 us_dist_total[j]=0;
Nanaud 24:be2b2be6907b 39 }
Nanaud 24:be2b2be6907b 40
Nanaud 25:869b1c1f51a7 41 i=0;
Nanaud 24:be2b2be6907b 42 }
Nanaud 24:be2b2be6907b 43 }
Nanaud 20:7d206773f39e 44
Nanaud 20:7d206773f39e 45
Nanaud 20:7d206773f39e 46 void captUS_trig()
Nanaud 20:7d206773f39e 47 {
Nanaud 25:869b1c1f51a7 48 captUS_convToDist();
Nanaud 20:7d206773f39e 49
Nanaud 25:869b1c1f51a7 50 captUS_moyenne();
Nanaud 24:be2b2be6907b 51
Nanaud 25:869b1c1f51a7 52 if((objRecule[indiceStrategie]==0) && (action == 2)) {
Nanaud 25:869b1c1f51a7 53 if ((us_dist_moy[5] >= (DISTLIM-OFFSET)) && (us_dist_moy[0] >= DISTLIM) && (us_dist_moy[1] >= (DISTLIM-OFFSET))) {
Nanaud 25:869b1c1f51a7 54 us_libre = 1;
Nanaud 21:e5f0f5abb5ae 55 }
Nanaud 1:2fe8c402ee79 56
Nanaud 21:e5f0f5abb5ae 57 else {
Nanaud 20:7d206773f39e 58 mot_dis();
Nanaud 25:869b1c1f51a7 59 us_rbt_restart = 1;
Nanaud 25:869b1c1f51a7 60 us_libre=0;
Nanaud 20:7d206773f39e 61 }
Nanaud 20:7d206773f39e 62 }
Nanaud 20:7d206773f39e 63
Nanaud 25:869b1c1f51a7 64 else if((objRecule[indiceStrategie]==1) && (action == 2)) {
Nanaud 25:869b1c1f51a7 65 if ((us_dist_moy[2] >= (DISTLIM-OFFSET)) && (us_dist_moy[3] >= DISTLIM) && (us_dist_moy[4] >= (DISTLIM-OFFSET))) {
Nanaud 25:869b1c1f51a7 66 us_libre=1;
Nanaud 21:e5f0f5abb5ae 67 }
Nanaud 20:7d206773f39e 68
Nanaud 21:e5f0f5abb5ae 69 else {
Nanaud 20:7d206773f39e 70 mot_dis();
Nanaud 25:869b1c1f51a7 71 us_rbt_restart = 1;
Nanaud 25:869b1c1f51a7 72 us_libre=0;
Nanaud 20:7d206773f39e 73 }
Nanaud 20:7d206773f39e 74 }
Nanaud 20:7d206773f39e 75
Nanaud 22:f891c2bce091 76 TimUS.reset();
Nanaud 0:dc036b67c87c 77 trigger=1;
Nanaud 0:dc036b67c87c 78 wait(0.00002);
Nanaud 0:dc036b67c87c 79 trigger=0;
Nanaud 0:dc036b67c87c 80 }
Nanaud 0:dc036b67c87c 81
Nanaud 20:7d206773f39e 82 void echoRise1()
Nanaud 20:7d206773f39e 83 {
Nanaud 25:869b1c1f51a7 84 if(us_verif2[1][0] == false) {
Nanaud 22:f891c2bce091 85 us_high[0]=TimUS.read_us();
Nanaud 25:869b1c1f51a7 86 us_verif2[0][0] = true;
Nanaud 21:e5f0f5abb5ae 87 }
Nanaud 20:7d206773f39e 88 }
Nanaud 21:e5f0f5abb5ae 89
Nanaud 20:7d206773f39e 90 void echoFall1()
Nanaud 20:7d206773f39e 91 {
Nanaud 25:869b1c1f51a7 92 if(us_verif2[0][0] == true) {
Nanaud 22:f891c2bce091 93 us_low[0]=TimUS.read_us();
Nanaud 25:869b1c1f51a7 94 us_verif2[1][0] = true;
Nanaud 21:e5f0f5abb5ae 95 }
Nanaud 20:7d206773f39e 96 }
Nanaud 20:7d206773f39e 97
Nanaud 20:7d206773f39e 98 void echoRise2()
Nanaud 20:7d206773f39e 99 {
Nanaud 25:869b1c1f51a7 100 if(us_verif2[1][1] == false) {
Nanaud 22:f891c2bce091 101 us_high[1]=TimUS.read_us();
Nanaud 25:869b1c1f51a7 102 us_verif2[0][1] = true;
Nanaud 21:e5f0f5abb5ae 103 }
Nanaud 20:7d206773f39e 104 }
Nanaud 21:e5f0f5abb5ae 105
Nanaud 20:7d206773f39e 106 void echoFall2()
Nanaud 20:7d206773f39e 107 {
Nanaud 25:869b1c1f51a7 108 if(us_verif2[0][1] == true) {
Nanaud 22:f891c2bce091 109 us_low[1]=TimUS.read_us();
Nanaud 25:869b1c1f51a7 110 us_verif2[1][1] = true;
Nanaud 21:e5f0f5abb5ae 111 }
Nanaud 20:7d206773f39e 112 }
Nanaud 20:7d206773f39e 113
Nanaud 20:7d206773f39e 114 void echoRise3()
Nanaud 20:7d206773f39e 115 {
Nanaud 25:869b1c1f51a7 116 if(us_verif2[1][2] == false) {
Nanaud 22:f891c2bce091 117 us_high[2]=TimUS.read_us();
Nanaud 25:869b1c1f51a7 118 us_verif2[0][2] = true;
Nanaud 21:e5f0f5abb5ae 119 }
Nanaud 20:7d206773f39e 120 }
Nanaud 21:e5f0f5abb5ae 121
Nanaud 20:7d206773f39e 122 void echoFall3()
Nanaud 20:7d206773f39e 123 {
Nanaud 25:869b1c1f51a7 124 if(us_verif2[0][2] == true) {
Nanaud 22:f891c2bce091 125 us_low[2]=TimUS.read_us();
Nanaud 25:869b1c1f51a7 126 us_verif2[1][2] = true;
Nanaud 21:e5f0f5abb5ae 127 }
Nanaud 20:7d206773f39e 128 }
Nanaud 0:dc036b67c87c 129
Nanaud 20:7d206773f39e 130 void echoRise4()
Nanaud 20:7d206773f39e 131 {
Nanaud 25:869b1c1f51a7 132 if(us_verif2[1][3] == false) {
Nanaud 22:f891c2bce091 133 us_high[3]=TimUS.read_us();
Nanaud 25:869b1c1f51a7 134 us_verif2[0][3] = true;
Nanaud 21:e5f0f5abb5ae 135 }
Nanaud 20:7d206773f39e 136 }
Nanaud 21:e5f0f5abb5ae 137
Nanaud 20:7d206773f39e 138 void echoFall4()
Nanaud 20:7d206773f39e 139 {
Nanaud 25:869b1c1f51a7 140 if(us_verif2[0][3] == true) {
Nanaud 22:f891c2bce091 141 us_low[3]=TimUS.read_us();
Nanaud 25:869b1c1f51a7 142 us_verif2[1][3] = true;
Nanaud 21:e5f0f5abb5ae 143 }
Nanaud 20:7d206773f39e 144 }
Nanaud 0:dc036b67c87c 145
Nanaud 20:7d206773f39e 146 void echoRise5()
Nanaud 20:7d206773f39e 147 {
Nanaud 25:869b1c1f51a7 148 if(us_verif2[1][4] == false) {
Nanaud 22:f891c2bce091 149 us_high[4]=TimUS.read_us();
Nanaud 25:869b1c1f51a7 150 us_verif2[0][4] = true;
Nanaud 21:e5f0f5abb5ae 151 }
Nanaud 20:7d206773f39e 152 }
Nanaud 21:e5f0f5abb5ae 153
Nanaud 20:7d206773f39e 154 void echoFall5()
Nanaud 20:7d206773f39e 155 {
Nanaud 25:869b1c1f51a7 156 if(us_verif2[0][4] == true) {
Nanaud 22:f891c2bce091 157 us_low[4]=TimUS.read_us();
Nanaud 25:869b1c1f51a7 158 us_verif2[1][4] = true;
Nanaud 21:e5f0f5abb5ae 159 }
Nanaud 20:7d206773f39e 160 }
Nanaud 20:7d206773f39e 161
Nanaud 20:7d206773f39e 162 void echoRise6()
Nanaud 20:7d206773f39e 163 {
Nanaud 25:869b1c1f51a7 164 if(us_verif2[1][5] == false) {
Nanaud 22:f891c2bce091 165 us_high[5]=TimUS.read_us();
Nanaud 25:869b1c1f51a7 166 us_verif2[0][5] = true;
Nanaud 21:e5f0f5abb5ae 167 }
Nanaud 20:7d206773f39e 168 }
Nanaud 21:e5f0f5abb5ae 169
Nanaud 20:7d206773f39e 170 void echoFall6()
Nanaud 20:7d206773f39e 171 {
Nanaud 25:869b1c1f51a7 172 if(us_verif2[0][5] == true) {
Nanaud 22:f891c2bce091 173 us_low[5]=TimUS.read_us();
Nanaud 25:869b1c1f51a7 174 us_verif2[1][5] = true;
Nanaud 21:e5f0f5abb5ae 175 }
Nanaud 20:7d206773f39e 176 }
Nanaud 20:7d206773f39e 177
Nanaud 0:dc036b67c87c 178
Nanaud 25:869b1c1f51a7 179 void captUS_convToDist()
Nanaud 20:7d206773f39e 180 {
Nanaud 20:7d206773f39e 181 /**************************************
Nanaud 20:7d206773f39e 182 * Nous convertisons grâce au valeur *
Nanaud 20:7d206773f39e 183 * qui sont retournées par echoRiseX *
Nanaud 20:7d206773f39e 184 * et echoFallx *
Nanaud 20:7d206773f39e 185 **************************************/
Nanaud 20:7d206773f39e 186
Nanaud 21:e5f0f5abb5ae 187 for(int i = 0; i<6; i++) {
Nanaud 25:869b1c1f51a7 188 if(us_verif2[0][i] == true && us_verif2[1][i] == true)
Nanaud 25:869b1c1f51a7 189 us_dist[i] = (int) (((us_low[i] - us_high[i])*340)/1000); //conversion en distance(mm)
Nanaud 25:869b1c1f51a7 190 else
Nanaud 25:869b1c1f51a7 191 us_dist[i] = LARGEUR_TAB;
Nanaud 20:7d206773f39e 192 }
Nanaud 20:7d206773f39e 193 }