A.I.Mergence / Mbed OS mbed-perceptron-2

évitement d obstacles

Dependencies:   Motor 16_Channel_Analog VL53L0X

main.cpp@2:f3ca81bbfabf, 2020-07-17 (annotated)

Committer:
sylvaingauthier
Date:
Fri Jul 17 14:04:04 2020 +0000
Revision:
2:f3ca81bbfabf
Parent:
1:53b992a47b28 
test

Who changed what in which revision?

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