AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode

Code principal du projet

Dependencies:   mbed DRV8825

captUS.cpp@26:bb2b778bd351, 2020-11-03 (annotated)

Committer:
Nanaud
Date:
Tue Nov 03 16:29:13 2020 +0000
Revision:
26:bb2b778bd351
Parent:
25:869b1c1f51a7 
UI

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