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 MotorDC 16_Channel_Analog VL53L0X
main.cpp@1:ba1ca7a99573, 2020-07-17 (annotated)
- Committer:
- evgeniik
- Date:
- Fri Jul 17 12:45:32 2020 +0000
- Revision:
- 1:ba1ca7a99573
- Parent:
- 0:dec1366627ac
test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
evgeniik | 0:dec1366627ac | 1 | #include "mbed.h" |
evgeniik | 1:ba1ca7a99573 | 2 | #include "VL53L0X.h" |
evgeniik | 1:ba1ca7a99573 | 3 | #include "Multi_16.h" |
evgeniik | 1:ba1ca7a99573 | 4 | #include "Motor.h" |
evgeniik | 1:ba1ca7a99573 | 5 | |
evgeniik | 1:ba1ca7a99573 | 6 | Motor m1(A2, A3, A5); // pwm, fwd, rev |
evgeniik | 1:ba1ca7a99573 | 7 | Motor m2(A1, D13, A0); // pwm, fwd, rev |
evgeniik | 1:ba1ca7a99573 | 8 | |
evgeniik | 1:ba1ca7a99573 | 9 | #define range_addr (0x56) |
evgeniik | 1:ba1ca7a99573 | 10 | #define mux_range 7 |
evgeniik | 1:ba1ca7a99573 | 11 | |
evgeniik | 1:ba1ca7a99573 | 12 | #define range1_XSHUT D2 |
evgeniik | 1:ba1ca7a99573 | 13 | #define VL53L0_I2C_SDA PA_10 |
evgeniik | 1:ba1ca7a99573 | 14 | #define VL53L0_I2C_SCL PA_9 |
evgeniik | 1:ba1ca7a99573 | 15 | Serial pc(SERIAL_TX, SERIAL_RX); |
evgeniik | 1:ba1ca7a99573 | 16 | |
evgeniik | 1:ba1ca7a99573 | 17 | Serial blut(PB_6, PB_7);//Tx, Rx |
evgeniik | 1:ba1ca7a99573 | 18 | |
evgeniik | 1:ba1ca7a99573 | 19 | static DevI2C devI2c(VL53L0_I2C_SDA, VL53L0_I2C_SCL); |
evgeniik | 1:ba1ca7a99573 | 20 | static DigitalOut shutdown_pin(range1_XSHUT); |
evgeniik | 1:ba1ca7a99573 | 21 | static VL53L0X tof(&devI2c, &shutdown_pin); |
evgeniik | 1:ba1ca7a99573 | 22 | |
evgeniik | 1:ba1ca7a99573 | 23 | typedef struct Tofs { |
evgeniik | 1:ba1ca7a99573 | 24 | int nmb; |
evgeniik | 1:ba1ca7a99573 | 25 | int n_tof_found; |
evgeniik | 1:ba1ca7a99573 | 26 | bool one; |
evgeniik | 1:ba1ca7a99573 | 27 | bool presence[7]; |
evgeniik | 1:ba1ca7a99573 | 28 | uint32_t address[7]; |
evgeniik | 1:ba1ca7a99573 | 29 | uint32_t dist[7]; |
evgeniik | 1:ba1ca7a99573 | 30 | uint32_t correction[7]; |
evgeniik | 1:ba1ca7a99573 | 31 | } MultiTof; |
evgeniik | 1:ba1ca7a99573 | 32 | |
evgeniik | 1:ba1ca7a99573 | 33 | MultiTof mt; |
evgeniik | 1:ba1ca7a99573 | 34 | |
evgeniik | 1:ba1ca7a99573 | 35 | void verif_tof(void); |
evgeniik | 1:ba1ca7a99573 | 36 | Ticker time_up; |
evgeniik | 1:ba1ca7a99573 | 37 | |
evgeniik | 1:ba1ca7a99573 | 38 | |
evgeniik | 1:ba1ca7a99573 | 39 | |
evgeniik | 1:ba1ca7a99573 | 40 | // Capteurs TOF /*Contruct the sensors*/ |
evgeniik | 1:ba1ca7a99573 | 41 | //static DigitalOut shutdown_pin(range1_XSHUT); |
evgeniik | 1:ba1ca7a99573 | 42 | //static VL53L0X tof(&devI2c, &shutdown_pin); |
evgeniik | 1:ba1ca7a99573 | 43 | |
evgeniik | 1:ba1ca7a99573 | 44 | |
evgeniik | 1:ba1ca7a99573 | 45 | |
evgeniik | 1:ba1ca7a99573 | 46 | // Multiplexeur |
evgeniik | 1:ba1ca7a99573 | 47 | static Mux mux(D9,D10,D11,D12,D3); // pour L432KC |
evgeniik | 1:ba1ca7a99573 | 48 | //static Mux mux(D2,D3,D4,D5,D6); |
evgeniik | 1:ba1ca7a99573 | 49 | |
evgeniik | 1:ba1ca7a99573 | 50 | int init_tof(int i){ |
evgeniik | 1:ba1ca7a99573 | 51 | int status = 0; |
evgeniik | 1:ba1ca7a99573 | 52 | mux.set_channel(i); |
evgeniik | 1:ba1ca7a99573 | 53 | //tof.VL53L0X_off(); |
evgeniik | 1:ba1ca7a99573 | 54 | |
evgeniik | 1:ba1ca7a99573 | 55 | tof.default_addr(); |
evgeniik | 1:ba1ca7a99573 | 56 | status = tof.init_sensor(range_addr); |
evgeniik | 1:ba1ca7a99573 | 57 | if (status == VL53L0X_ERROR_NONE){ |
evgeniik | 1:ba1ca7a99573 | 58 | mt.one = true; |
evgeniik | 1:ba1ca7a99573 | 59 | mt.presence[i] = true; |
evgeniik | 1:ba1ca7a99573 | 60 | //tof.VL53L0X_off(); |
evgeniik | 1:ba1ca7a99573 | 61 | //tof.VL53L0X_on(); |
evgeniik | 1:ba1ca7a99573 | 62 | //mt.address[i] = 0x52; |
evgeniik | 1:ba1ca7a99573 | 63 | mt.address[i] = range_addr; |
evgeniik | 1:ba1ca7a99573 | 64 | return 0; // init ok (address was default = 0x52 and now address = 0x56) |
evgeniik | 1:ba1ca7a99573 | 65 | } |
evgeniik | 1:ba1ca7a99573 | 66 | else{ |
evgeniik | 1:ba1ca7a99573 | 67 | tof.set_addr_bib(range_addr); |
evgeniik | 1:ba1ca7a99573 | 68 | if (status == VL53L0X_ERROR_NONE){ |
evgeniik | 1:ba1ca7a99573 | 69 | mt.one = true; |
evgeniik | 1:ba1ca7a99573 | 70 | mt.presence[i] = true; |
evgeniik | 1:ba1ca7a99573 | 71 | //tof.VL53L0X_off(); |
evgeniik | 1:ba1ca7a99573 | 72 | //tof.VL53L0X_on(); |
evgeniik | 1:ba1ca7a99573 | 73 | //mt.address[i] = 0x52; |
evgeniik | 1:ba1ca7a99573 | 74 | mt.address[i] = range_addr; |
evgeniik | 1:ba1ca7a99573 | 75 | return 0; // init ok (address was 0x56) |
evgeniik | 1:ba1ca7a99573 | 76 | } |
evgeniik | 1:ba1ca7a99573 | 77 | else return -1; // not detected |
evgeniik | 1:ba1ca7a99573 | 78 | } |
evgeniik | 1:ba1ca7a99573 | 79 | } |
evgeniik | 1:ba1ca7a99573 | 80 | |
evgeniik | 1:ba1ca7a99573 | 81 | int init_tofs() { |
evgeniik | 1:ba1ca7a99573 | 82 | int etat = 0; |
evgeniik | 1:ba1ca7a99573 | 83 | tof.VL53L0X_off(); |
evgeniik | 1:ba1ca7a99573 | 84 | |
evgeniik | 1:ba1ca7a99573 | 85 | |
evgeniik | 1:ba1ca7a99573 | 86 | for (int i = 0; i<mt.nmb; i++){ |
evgeniik | 1:ba1ca7a99573 | 87 | etat = init_tof(i); |
evgeniik | 1:ba1ca7a99573 | 88 | if (etat == 0){ |
evgeniik | 1:ba1ca7a99573 | 89 | mt.n_tof_found++; |
evgeniik | 1:ba1ca7a99573 | 90 | if (i<9) printf("+\t"); |
evgeniik | 1:ba1ca7a99573 | 91 | else printf("+\t"); |
evgeniik | 1:ba1ca7a99573 | 92 | } |
evgeniik | 1:ba1ca7a99573 | 93 | else{ |
evgeniik | 1:ba1ca7a99573 | 94 | if (i<9) printf("-\t"); |
evgeniik | 1:ba1ca7a99573 | 95 | else printf("-\t"); |
evgeniik | 1:ba1ca7a99573 | 96 | } |
evgeniik | 1:ba1ca7a99573 | 97 | } |
evgeniik | 1:ba1ca7a99573 | 98 | |
evgeniik | 1:ba1ca7a99573 | 99 | printf("\n"); |
evgeniik | 1:ba1ca7a99573 | 100 | if (mt.one) { printf("At least 1 is found. Nmb : %d\n", mt.n_tof_found);tof.VL53L0X_off();tof.VL53L0X_on(); return 0; } |
evgeniik | 1:ba1ca7a99573 | 101 | else return -1; |
evgeniik | 1:ba1ca7a99573 | 102 | } |
evgeniik | 1:ba1ca7a99573 | 103 | |
evgeniik | 1:ba1ca7a99573 | 104 | |
evgeniik | 1:ba1ca7a99573 | 105 | bool seuil(float val, float s){ |
evgeniik | 1:ba1ca7a99573 | 106 | return val < s; |
evgeniik | 1:ba1ca7a99573 | 107 | } |
evgeniik | 1:ba1ca7a99573 | 108 | |
evgeniik | 0:dec1366627ac | 109 | |
evgeniik | 1:ba1ca7a99573 | 110 | int main() |
evgeniik | 1:ba1ca7a99573 | 111 | { |
evgeniik | 1:ba1ca7a99573 | 112 | int status; |
evgeniik | 1:ba1ca7a99573 | 113 | uint32_t mesure; |
evgeniik | 1:ba1ca7a99573 | 114 | // Initialisation de MultiTof |
evgeniik | 1:ba1ca7a99573 | 115 | mt.correction[0] = 1.11; |
evgeniik | 1:ba1ca7a99573 | 116 | mt.correction[1] = 1.08; |
evgeniik | 1:ba1ca7a99573 | 117 | mt.correction[2] = 1; |
evgeniik | 1:ba1ca7a99573 | 118 | mt.correction[3] = 1.05; |
evgeniik | 1:ba1ca7a99573 | 119 | mt.correction[4] = 1; |
evgeniik | 1:ba1ca7a99573 | 120 | mt.correction[5] = 1.18; |
evgeniik | 1:ba1ca7a99573 | 121 | mt.correction[6] = 1; |
evgeniik | 1:ba1ca7a99573 | 122 | |
evgeniik | 1:ba1ca7a99573 | 123 | mt.nmb = mux_range; |
evgeniik | 1:ba1ca7a99573 | 124 | mt.n_tof_found = 0; |
evgeniik | 1:ba1ca7a99573 | 125 | mt.one = false; |
evgeniik | 1:ba1ca7a99573 | 126 | for (int i=0; i<mt.nmb; i++){ |
evgeniik | 1:ba1ca7a99573 | 127 | mt.presence[i] = false; // presence du capteur |
evgeniik | 1:ba1ca7a99573 | 128 | mt.address[i] = 0x52; // address (default = 0x52) |
evgeniik | 1:ba1ca7a99573 | 129 | mt.dist[i] = 9999; // distance |
evgeniik | 1:ba1ca7a99573 | 130 | } |
evgeniik | 1:ba1ca7a99573 | 131 | |
evgeniik | 1:ba1ca7a99573 | 132 | status = init_tofs(); |
evgeniik | 1:ba1ca7a99573 | 133 | |
evgeniik | 1:ba1ca7a99573 | 134 | for(int i=0; i<mt.nmb; i++) { printf("%x\t", mt.address[i]); } |
evgeniik | 1:ba1ca7a99573 | 135 | printf("\n"); |
evgeniik | 1:ba1ca7a99573 | 136 | |
evgeniik | 1:ba1ca7a99573 | 137 | printf("status = %d \n", status); |
evgeniik | 1:ba1ca7a99573 | 138 | |
evgeniik | 1:ba1ca7a99573 | 139 | //time_up.attach(&verif_tof, 20); |
evgeniik | 0:dec1366627ac | 140 | |
evgeniik | 1:ba1ca7a99573 | 141 | if (!status){ |
evgeniik | 1:ba1ca7a99573 | 142 | while(1){ |
evgeniik | 1:ba1ca7a99573 | 143 | Timer t; |
evgeniik | 1:ba1ca7a99573 | 144 | t.start(); |
evgeniik | 1:ba1ca7a99573 | 145 | for(int i=0; i<mux_range; i++){ |
evgeniik | 1:ba1ca7a99573 | 146 | if (i == 1 || i == 3 || i == 6){ |
evgeniik | 1:ba1ca7a99573 | 147 | mux.set_channel(i); |
evgeniik | 1:ba1ca7a99573 | 148 | if (mt.presence[i]){ |
evgeniik | 1:ba1ca7a99573 | 149 | if (mt.address[i] == range_addr){ |
evgeniik | 1:ba1ca7a99573 | 150 | tof.set_addr_bib(mt.address[i]); |
evgeniik | 1:ba1ca7a99573 | 151 | status = tof.get_distance(&mesure); |
evgeniik | 1:ba1ca7a99573 | 152 | if (status == VL53L0X_ERROR_NONE) { mt.dist[i] = mesure*mt.correction[i]; printf("%d\t", mt.dist[i]); } |
evgeniik | 1:ba1ca7a99573 | 153 | else { |
evgeniik | 1:ba1ca7a99573 | 154 | tof.default_addr(); |
evgeniik | 1:ba1ca7a99573 | 155 | status = tof.get_distance(&mesure); |
evgeniik | 1:ba1ca7a99573 | 156 | if (status == VL53L0X_ERROR_NONE){ mt.dist[i] = mesure*mt.correction[i]; mt.presence[i] = true; mt.address[i] = 0x52; printf("%d\t", mt.dist[i]); } |
evgeniik | 1:ba1ca7a99573 | 157 | else if (status == -6) { mt.dist[i]+=200; printf("%d*\t", mt.dist[i]); } //printf("OoR\t"); |
evgeniik | 1:ba1ca7a99573 | 158 | else { printf("--\t"); mt.presence[i] = false; } |
evgeniik | 1:ba1ca7a99573 | 159 | } |
evgeniik | 1:ba1ca7a99573 | 160 | } |
evgeniik | 1:ba1ca7a99573 | 161 | else{ |
evgeniik | 1:ba1ca7a99573 | 162 | tof.default_addr(); |
evgeniik | 1:ba1ca7a99573 | 163 | status = tof.get_distance(&mesure); |
evgeniik | 1:ba1ca7a99573 | 164 | if (status == VL53L0X_ERROR_NONE){ mt.dist[i] = mesure*mt.correction[i]; mt.presence[i] = true; mt.address[i] = 0x52; printf("%d\t", mt.dist[i]); } |
evgeniik | 1:ba1ca7a99573 | 165 | else if (status == -6) { mt.dist[i]+=200; printf("%d*\t", mt.dist[i]); }//printf("OoR\t"); |
evgeniik | 1:ba1ca7a99573 | 166 | else { printf("--\t"); mt.presence[i] = false; } |
evgeniik | 1:ba1ca7a99573 | 167 | } |
evgeniik | 1:ba1ca7a99573 | 168 | } |
evgeniik | 1:ba1ca7a99573 | 169 | else{ |
evgeniik | 1:ba1ca7a99573 | 170 | printf("--\t"); |
evgeniik | 1:ba1ca7a99573 | 171 | tof.default_addr(); |
evgeniik | 1:ba1ca7a99573 | 172 | status = init_tof(i);//tof.init_sensor(range_addr); |
evgeniik | 1:ba1ca7a99573 | 173 | } |
evgeniik | 1:ba1ca7a99573 | 174 | } |
evgeniik | 1:ba1ca7a99573 | 175 | } |
evgeniik | 1:ba1ca7a99573 | 176 | mt.one = false; |
evgeniik | 1:ba1ca7a99573 | 177 | for(int i=0; i<mt.nmb; i++){ if (mt.presence[i] == true) {mt.one = true; break;} } |
evgeniik | 1:ba1ca7a99573 | 178 | t.stop(); printf("%f \t one = %d \n", t.read(), mt.one); |
evgeniik | 1:ba1ca7a99573 | 179 | for(int i=0; i<mux_range; i++){ |
evgeniik | 1:ba1ca7a99573 | 180 | if (mt.dist[i] > 1000) mt.dist[i] = 1000; |
evgeniik | 1:ba1ca7a99573 | 181 | } |
evgeniik | 1:ba1ca7a99573 | 182 | //wait(0.1); |
evgeniik | 1:ba1ca7a99573 | 183 | //blut.printf("TOF/|%d|%d|%d|%d|%d|%d|%d/TOF", 200, mt.dist[1], mt.dist[2], mt.dist[3], mt.dist[4], mt.dist[5], mt.dist[6]); |
evgeniik | 1:ba1ca7a99573 | 184 | blut.printf("t/|%d||%d||%d||%d||%d||%d||%d|*/t", mt.dist[0], mt.dist[1], mt.dist[2], mt.dist[3], mt.dist[4], mt.dist[5], mt.dist[6]); |
evgeniik | 1:ba1ca7a99573 | 185 | |
evgeniik | 1:ba1ca7a99573 | 186 | |
evgeniik | 1:ba1ca7a99573 | 187 | /////////////lineaire////////////////////// |
evgeniik | 1:ba1ca7a99573 | 188 | // Start pour Sylvain |
evgeniik | 1:ba1ca7a99573 | 189 | float c = mt.dist[1]; |
evgeniik | 1:ba1ca7a99573 | 190 | float g = mt.dist[3]; |
evgeniik | 1:ba1ca7a99573 | 191 | float d = mt.dist[6]; |
evgeniik | 1:ba1ca7a99573 | 192 | float v1 = (c + d + (1000-g))/3000; |
evgeniik | 1:ba1ca7a99573 | 193 | float v2 = (c + g + (1000-d))/3000; |
evgeniik | 1:ba1ca7a99573 | 194 | |
evgeniik | 1:ba1ca7a99573 | 195 | //if (v1 < 0.3) {v1 = 0;} |
evgeniik | 1:ba1ca7a99573 | 196 | //if (v2 < 0.3) {v2 = 0;} |
evgeniik | 1:ba1ca7a99573 | 197 | //if (v1 > 0.4) {v1 = 1;} @corentin |
evgeniik | 1:ba1ca7a99573 | 198 | //if (v2 > 0.4) {v2 = 1;} |
evgeniik | 1:ba1ca7a99573 | 199 | |
evgeniik | 1:ba1ca7a99573 | 200 | m1.speed(v1); |
evgeniik | 1:ba1ca7a99573 | 201 | m2.speed(v2); |
evgeniik | 1:ba1ca7a99573 | 202 | |
evgeniik | 1:ba1ca7a99573 | 203 | // End |
evgeniik | 1:ba1ca7a99573 | 204 | |
evgeniik | 1:ba1ca7a99573 | 205 | |
evgeniik | 1:ba1ca7a99573 | 206 | ///////////////0/////////1////////////// |
evgeniik | 1:ba1ca7a99573 | 207 | // Start pour Sylvain |
evgeniik | 1:ba1ca7a99573 | 208 | /*bool c = seuil(mt.dist[1], 500); |
evgeniik | 1:ba1ca7a99573 | 209 | bool g = seuil(mt.dist[3], 500); |
evgeniik | 1:ba1ca7a99573 | 210 | bool d = seuil(mt.dist[6], 500); |
evgeniik | 1:ba1ca7a99573 | 211 | |
evgeniik | 1:ba1ca7a99573 | 212 | bool v1 = !d + g*!c; |
evgeniik | 1:ba1ca7a99573 | 213 | bool v2 = !g*!c + !g*d + !c*d; |
evgeniik | 1:ba1ca7a99573 | 214 | |
evgeniik | 1:ba1ca7a99573 | 215 | printf("\ v1 = %d \t v2 = %d \n", (int)v1, (int)v2); |
evgeniik | 1:ba1ca7a99573 | 216 | m1.speed((int)v1); |
evgeniik | 1:ba1ca7a99573 | 217 | m2.speed((int)v2);*/ |
evgeniik | 1:ba1ca7a99573 | 218 | |
evgeniik | 1:ba1ca7a99573 | 219 | // End |
evgeniik | 1:ba1ca7a99573 | 220 | } // end while |
evgeniik | 1:ba1ca7a99573 | 221 | } |
evgeniik | 1:ba1ca7a99573 | 222 | |
evgeniik | 1:ba1ca7a99573 | 223 | } |
evgeniik | 1:ba1ca7a99573 | 224 |