fix center and improved example

Author: gbr1

examples/firmware_01/firmware_01.ino

@@ -97,7 +97,7 @@ void loop(){
     if (sensor_id>4){
       sensor_id=0;
     }
-  }
+  } 
 
   if (millis()-tmotor>10){
     tmotor=millis();

examples/sensor_tof/sensor_tof.ino

@@ -21,21 +21,22 @@ void loop() {
     updated = tof.update_rois();
 
     if (updated) {
+      /*
       Serial.println(" ");
       Serial.print("TOP=");
-      Serial.println(tof.top);
+      Serial.println(tof.getTop());
       Serial.print("BOT=");
-      Serial.println(tof.bottom);
+      Serial.println(tof.getBottom());
       Serial.print("LEFT=");
-      Serial.println(tof.left);
+      Serial.println(tof.getLeft());
       Serial.print("RIGHT=");
-      Serial.println(tof.right);
+      Serial.println(tof.getRight());
       Serial.print("CLEFT=");
-      Serial.println(tof.center_left);
+      Serial.println(tof.getCenterLeft());
       Serial.print("CRIGHT=");
-      Serial.println(tof.center_right);
+      Serial.println(tof.getCenterRight());
       Serial.print("CENTER=");
-      Serial.println(tof.center);
+      Serial.println(tof.getCenter());
       Serial.print("LEFT_MIN=");
       Serial.println(tof.get_min_range_left_mm());
       Serial.print("RIGHT_MIN=");
@@ -46,7 +47,24 @@ void loop() {
       Serial.println(tof.get_min_range_center_right_mm());
       Serial.print("CENTER_MIN=");
       Serial.println(tof.get_min_range_center_mm());
+      */
+      Serial.print("\t");
+      Serial.print(tof.getLeft());
+      Serial.print("\t");
+      Serial.print(tof.getLeftCenter());
+      Serial.print("\t");
+      Serial.print(tof.getCenter());
+      Serial.print("\t");
+      Serial.print(tof.getRightCenter());
+      Serial.print("\t");
+      Serial.print(tof.getRight());
+      Serial.print("\t");
+      Serial.print(tof.getTop());
+      Serial.print("\t");
+      Serial.print(tof.getBottom());
+      Serial.print("\n");
     }
+    
 
     delay(1000);
 }

src/sensor_tof_matrix.h

@@ -321,7 +321,7 @@ class SensorTofMatrix{
             int16_t _avg = 0;
             uint8_t n = 0;
 
-            for (int i=(_size==4?6:19); i < (_size==4?13:51) ;i+=_size) {
+            for (int i=(_size==4?5:19); i < (_size==4?13:51) ;i+=_size) {
                 _avg += results.distance_mm[i];
                 n++;
                 _avg += results.distance_mm[i+1];