File content as of revision 3:fc26045926d9:

#include "TOFs.h"
const uint8_t TOFs_id[N_TOFS] = { 0x20, 0x21, 0x22, 0x23};     //vettore contenente i nuovi address
DigitalOut TOFs_en[N_TOFS] = {D8, D9, D10, D12};                 //vettore contenente i pin di abilitazione dei laser
VL6180x TOFs[N_TOFS] = {
    VL6180x (PB_11, PB_10, TOF_ID_DEFAULT <<1),                     //->0x20
    VL6180x (PB_11, PB_10, TOF_ID_DEFAULT <<1),                     //->0x21
    VL6180x (PB_11, PB_10, TOF_ID_DEFAULT <<1),                     //->0x22
    VL6180x (PB_11, PB_10, TOF_ID_DEFAULT <<1)                      //->0x23
};

distanceSensors::distanceSensors(){
    acquireCount=0;
    frontDistance=0.0;
    retroDistance=0.0;
    
    for (int i = 0; i < N_TOFS; i++) // ...initialize it
    {
        TOFs_val[i] = 0;
        TOFs_val_prec[i] = 0;
        Offset[i] = 0;

    }
    

    
    
    
}

void distanceSensors::TOFs_init()
{    
  uint8_t retaddr;  
  for( int k = 0; k < N_TOFS; k++) 
  {               
     TOFs_en[k] = 0;                                //all off
  }
  wait(0.2);                                       // mi assicuro che tutti i livelli logici si siano abbassati 
  for ( int i = 0 ; i < N_TOFS ; i++ )
  {       
     TOFs_en[i] = 1;   
     wait(0.1);                                     // mi assicuro che il livello logici si sia alzato
     if(TOFs[i].VL6180xInit() != 0)
     {                       //VUOL DIRE CHE IL DISPOSITIVO E' STATO GIA' INIZIALIZZATO IN PRECEDENZA
            printf("\nFAILED TO INITALIZE TOF[%d] ",i); 
     }else
     {
           printf("\n OK INIT TOF[%d] \r\n  ",i); 
           TOFs[i].VL6180xDefautSettings(); //Load default settings to get started.
           printf("I want change the sensor default address 0x29 in 0x%x \r\n ", TOFs_id[i]);
           retaddr = TOFs[i].changeAddress(TOF_ID_DEFAULT, TOFs_id[i] );          //convert to new address 
           printf("New address is: 0x%x \r\n ",retaddr);
     }            
  }
  wait(0.5);
}



void distanceSensors::TOFs_offset(){
    
     int N = 20;
    
    for(int i=0;i<N;i++)
    {   
        TOFs_acquire1();

        Offset[0]=Offset[0]+TOFs_val[0];
        Offset[1]=Offset[1]+TOFs_val[1];
        Offset[2]=Offset[2]+TOFs_val[2];
        Offset[3]=Offset[3]+TOFs_val[3];
        printf("a: %2.1f %t b: %2.1f c: %2.1f %t d: %2.1f \r\n",Offset[0],Offset[1],Offset[2],Offset[3]);
        wait_ms(1);
      }
      
     Offset[0]=Offset[0]/(N-1);
     Offset[1]=Offset[1]/(N-1);
     Offset[2]=Offset[2]/(N-1);
     Offset[3]=Offset[3]/(N-1);
     printf("A: %2.1f %t B: %2.1f C: %2.1f %t D: %2.1f \r\n",Offset[0],Offset[1],Offset[2],Offset[3]);    
    TOFs_val_prec[0]=TOFs_val[0]-Offset[0]; 
    TOFs_val_prec[1]=TOFs_val[1]-Offset[1];
    TOFs_val_prec[2]=TOFs_val[2]-Offset[2]; 
    TOFs_val_prec[3]=TOFs_val[3]-Offset[3];
}



void distanceSensors::TOFs_acquire1()
{ 
   for( int i = 0; i < N_TOFS; i++)
   {               
      //  TOFs[i].getAmbientLight(GAIN_1);        //fa la compensazione prima
        TOFs_val[i] = TOFs[i].getDistance();    // //Get Distance and report in mm         
   }        
}


void distanceSensors::TOFs_acquireFiltr(){    
   
   //for( int i = 0; i < N_TOFS; i++)
   //{               
      //  TOFs[i].getAmbientLight(GAIN_1);        //fa la compensazione prima
        TOFs_val[acquireCount] = TOFs[acquireCount].getDistance();    // //Get Distance and report in mm      
        TOFs_val[acquireCount]= 0.05*(TOFs_val[acquireCount]-Offset[acquireCount])+0.95*(TOFs_val_prec[acquireCount]);
        
        //printf("distance: %d: %2.1f \r\n",acquireCount,TOFs_val[acquireCount]);

        
        //if (TOFs_val[acquireCount] > 40) TOFs_val[acquireCount] = 0.0;
        
        TOFs_val_prec[acquireCount]=TOFs_val[acquireCount];
        
        acquireCount ++; 
        if(acquireCount==4){
            frontDistance = TOFs_val[2]-TOFs_val[1];
            retroDistance = TOFs_val[3]-TOFs_val[0];
            acquireCount=0;
        }


   //}        

         
}  

float distanceSensors::getFrontDistance(){  
    return frontDistance;
}  

float distanceSensors::getRetroDistance(){  
    return retroDistance;
}