Diff: Robots/Rover/TOFs.cpp

Revision:

3:fc26045926d9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robots/Rover/TOFs.cpp	Wed Nov 06 10:57:51 2019 +0000
@@ -0,0 +1,132 @@
+#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;
+}