A.I.Mergence / Mbed 2 deprecated EvitObst

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?

UserRevisionLine numberNew 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