File content as of revision 2:f3ca81bbfabf:

#include "mbed.h"
#include "VL53L0X.h"
#include "Multi_16.h"
#include "Motor.h"
#include "perceptron.h"

#include <vector>
#include <iostream>
#include <stdio.h>

Motor m1(A2, A3, A5); // pwm, fwd, rev
Motor m2(A1, D13, A0); // pwm, fwd, rev

#define range_addr  (0x56)
#define mux_range 7

#define range1_XSHUT   D2
#define VL53L0_I2C_SDA   PA_10
#define VL53L0_I2C_SCL   PA_9
Serial pc(SERIAL_TX, SERIAL_RX);

Serial blut(PB_6, PB_7);//Tx, Rx

static DevI2C devI2c(VL53L0_I2C_SDA, VL53L0_I2C_SCL); 
static DigitalOut shutdown_pin(range1_XSHUT);
static VL53L0X tof(&devI2c, &shutdown_pin);

typedef struct Tofs {
   int      nmb;
   int      n_tof_found;
   bool     one;
   bool     presence[7];
   uint32_t address[7];
   uint32_t dist[7];
   uint32_t correction[7];
} MultiTof;

MultiTof mt;

void verif_tof(void);
Ticker time_up;
 


// Capteurs TOF /*Contruct the sensors*/ 
//static DigitalOut shutdown_pin(range1_XSHUT);
//static VL53L0X tof(&devI2c, &shutdown_pin);



// Multiplexeur
static Mux mux(D9,D10,D11,D12,D3); // pour L432KC
//static Mux mux(D2,D3,D4,D5,D6);

int init_tof(int i){
    int status = 0;
    mux.set_channel(i);
    //tof.VL53L0X_off();

    tof.default_addr();
    status = tof.init_sensor(range_addr);
    if (status == VL53L0X_ERROR_NONE){
        mt.one = true;
        mt.presence[i] = true;
        //tof.VL53L0X_off();
        //tof.VL53L0X_on();
        //mt.address[i]  = 0x52;
        mt.address[i]  = range_addr;
        return 0;                       // init ok (address was default = 0x52 and now address = 0x56)
    }
    else{
        tof.set_addr_bib(range_addr);
        if (status == VL53L0X_ERROR_NONE){
            mt.one = true;
            mt.presence[i] = true;
            //tof.VL53L0X_off();
            //tof.VL53L0X_on();
            //mt.address[i]  = 0x52;
            mt.address[i]  = range_addr;
            return 0;                  // init ok (address was 0x56)
        }
        else return -1;                 // not detected
    }
}

int init_tofs() {    
    int etat = 0;
    tof.VL53L0X_off();
    
    
    for (int i = 0; i<mt.nmb; i++){
        etat = init_tof(i);
        if (etat == 0){
            mt.n_tof_found++;
            if (i<9) printf("+\t");
            else printf("+\t");
        }
        else{
            if (i<9) printf("-\t");
            else printf("-\t");
        }
    }
    
    printf("\n");
    if (mt.one) { printf("At least 1 is found. Nmb : %d\n", mt.n_tof_found);tof.VL53L0X_off();tof.VL53L0X_on(); return 0; }
    else return -1;
}


bool seuil(float val, float s){
    return val < s;
}

 
int main()
{
    int status;
    uint32_t mesure;
    // Initialisation de MultiTof
    mt.correction[0] = 1.11;
    mt.correction[1] = 1.08;
    mt.correction[2] = 1;
    mt.correction[3] = 1.05;
    mt.correction[4] = 1;
    mt.correction[5] = 1.18;
    mt.correction[6] = 1;
    
    mt.nmb = mux_range;
    mt.n_tof_found  = 0;
    mt.one = false;
    for (int i=0; i<mt.nmb; i++){
        mt.presence[i] = false;      // presence du capteur
        mt.address[i]  = 0x52;       // address (default = 0x52)
        mt.dist[i]     = 9999;       // distance
    }
    
    status = init_tofs();
    
    for(int i=0; i<mt.nmb; i++) { printf("%x\t", mt.address[i]); }
    printf("\n");
    
    printf("status = %d \n", status);
    
    //time_up.attach(&verif_tof, 20);
    
    vector<float> Wg = {1,1,-1,-1};
    perceptron perceptronG(Wg);
    
    vector<float> Wd = {1,-1,-1,1};
    perceptron perceptronD(Wd);
    
    float seuil = 500;
    
    if (!status){
    while(1){
        Timer t;
        t.start();
        for(int i=0; i<mux_range; i++){
            if (i == 1 || i == 3 || i == 6){
            mux.set_channel(i);
            if (mt.presence[i]){
                if (mt.address[i] == range_addr){
                    tof.set_addr_bib(mt.address[i]);
                    status = tof.get_distance(&mesure);
                    if (status == VL53L0X_ERROR_NONE) { mt.dist[i] = mesure*mt.correction[i]; printf("%d\t", mt.dist[i]); }
                    else {
                        tof.default_addr();
                        status = tof.get_distance(&mesure);
                        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]); }
                        else if (status == -6) { mt.dist[i]+=200; printf("%d*\t", mt.dist[i]); } //printf("OoR\t");
                        else { printf("--\t"); mt.presence[i] = false; }
                    }
                }
                else{
                    tof.default_addr();
                    status = tof.get_distance(&mesure);
                    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]); }
                    else if (status == -6) { mt.dist[i]+=200; printf("%d*\t", mt.dist[i]); }//printf("OoR\t");
                    else { printf("--\t"); mt.presence[i] = false; }
                }
            }
            else{
                printf("--\t");
                tof.default_addr();
                status = init_tof(i);//tof.init_sensor(range_addr);
            }
            }
        }
        mt.one = false;
        for(int i=0; i<mt.nmb; i++){ if (mt.presence[i] == true) {mt.one = true; break;} }
        t.stop(); printf("%f \t one = %d \n", t.read(), mt.one);
        for(int i=0; i<mux_range; i++){
            if (mt.dist[i] > 1000) mt.dist[i] = 1000;
        }
        //wait(0.1);
        //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]);
        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]);
        
        
        /////////////lineaire//////////////////////
        // Start pour Sylvain
        float c = mt.dist[1];
        float g = mt.dist[3];
        float d = mt.dist[6];
        
        vector<int> X = {g<seuil,c<seuil,d<seuil};
        
        printf("X=%d,%d,%d\r\n",X[0],X[1],X[2]);
        
        int v1=perceptronG.predict(X);
        int v2=perceptronD.predict(X);
        
        if (v1 ==0){v1=-1;}
        if (v2 ==0){v2=-1;}
        
        m1.speed(v1);
        m2.speed(v2);
        
    } // end while
    }
 
}