File content as of revision 6:584653235830:

#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
};

TOFs::TOFs(){
    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 TOFs::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 TOFs::TOFs_offset(){
    
     int N = 50;
    
    for(int i=0;i<N;i++)
    {   
        TOFs_acquire1();
        //printf("Stampo i valori dei tof durante calcono offset --- tv1: %2.1f %t tv1: %2.1f tv1: %2.1f %t tv1: %2.1f \r\n",TOFs_val[0],TOFs_val[1],TOFs_val[2],TOFs_val[3]);

        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("o1: %2.1f %t o2: %2.1f o3: %2.1f %t o4: %2.1f \r\n",Offset[0],Offset[1],Offset[2],Offset[3]);
        ThisThread::sleep_for(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 TOFs::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  
        ThisThread::sleep_for(5);       
   }        
}


void TOFs::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[0]-TOFs_val[1];
            retroDistance = TOFs_val[3]-TOFs_val[2];
            acquireCount=0;
        }


   //}        

         
}  

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

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

float TOFs::getLaserFrontDx(){  
    return TOFs_val[1];
} 

float TOFs::getLaserFrontSx(){  
    return TOFs_val[0];
} 

float TOFs::getLaserRetroDx(){  
    return TOFs_val[2];
} 

float TOFs::getLaserRetroSx(){  
    return TOFs_val[3];
}