Median of 3 filter for multiple time of flight sensors translated from arduino by pololu

Dependencies:   mbed

Dependents:   wheelchaircontrol wheelchaircontrol2 wheelchaircontrol3 wheelchaircontrol4 ... more

Revision:
1:0038ad0a63af
Parent:
0:617f20c6b21d
Child:
2:ee9bc2e6dffd
Child:
3:0d2f2f31469b
--- a/VL53L1X.cpp	Thu Aug 09 22:06:49 2018 +0000
+++ b/VL53L1X.cpp	Mon Oct 08 23:01:21 2018 +0000
@@ -32,7 +32,7 @@
 {
   writeReg(I2C_SLAVE__DEVICE_ADDRESS, new_addr & 0x7F);
   wait(.01);
-  printf("%x\r\n", readReg(I2C_SLAVE__DEVICE_ADDRESS));
+  ////printf("%x\r\n", readReg(I2C_SLAVE__DEVICE_ADDRESS));
   address = new_addr << 1;
 }
  
@@ -44,7 +44,7 @@
 {
   // check model ID and module type registers (values specified in datasheet)
   int tempRegister = readReg16Bit(IDENTIFICATION__MODEL_ID);
-  printf("temporary %x\r\n", tempRegister);
+  //printf("temporary %x\r\n", tempRegister);
   if (tempRegister != 0xEACC) {  
     return false; 
   }
@@ -59,10 +59,10 @@
  
   startTimeout();
   int firmware = (readReg16Bit(FIRMWARE__SYSTEM_STATUS));
-  printf("firmware : %x\r\n", firmware);
+  //printf("firmware : %x\r\n", firmware);
   while ((readReg(FIRMWARE__SYSTEM_STATUS) & 0x01) == 0)
   {
-    printf("stuck\r\n");
+    //printf("stuck\r\n");
     if (checkTimeoutExpired())
     {
       did_timeout = true;
@@ -151,7 +151,7 @@
  
   // default to long range, 50 ms timing budget
   // note that this is different than what the API defaults to
-  setDistanceMode(Short);
+  setDistanceMode(Long);
   setMeasurementTimingBudget(50000);
  
   // VL53L1_StaticInit() end