Diff: TOFs/TOFs.cpp

Revision:

10:62e9b61ed1ad

Parent:

9:03e93b86a922

Child:

11:d93fbd8e02e9

--- a/TOFs/TOFs.cpp	Mon Dec 20 10:30:08 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,150 +0,0 @@
-#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];
-}