Added more code for side and angled sensor

Dependencies:   QEI2 PID Watchdog VL53L1X_Filter BNOWrapper ros_lib_kinetic

Revision:
32:fb26baa75d44
Parent:
31:06f2362caf12
Child:
33:f3585571f11e
--- a/wheelchair.cpp	Mon Jul 01 23:20:48 2019 +0000
+++ b/wheelchair.cpp	Mon Jul 01 23:35:39 2019 +0000
@@ -51,10 +51,10 @@
     for(int i = 0; i < 6; i++)                                              // reads from the ToF Sensors
     {
         ToFV[i] = (*(ToF+i))->readFromOneSensor();
-        out->printf("%d ", ToFV[i]);
+        //out->printf("%d ", ToFV[i]);
     }  
   
-        out->printf("\r\n");
+        //out->printf("\r\n");
     
     k++;
     
@@ -71,13 +71,13 @@
     out->printf("\r\n");*/
 //    statistics LFTStats(ToFDataPointer1, 99, 1);
 //    statistics RFTStats(ToFDataPointer1, 99, 1);  
-    out->printf("Right Mean: %f ", RFTStats.mean());
-    out->printf("Std Dev: % f", RFTStats.stdev());
+    //out->printf("Right Mean: %f ", RFTStats.mean());
+    //out->printf("Std Dev: % f", RFTStats.stdev());
     outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev();
-    out->printf("Left Mean: %f ", LFTStats.mean());
-    out->printf("Std Dev: %f ", LFTStats.stdev());
+    //out->printf("Left Mean: %f ", LFTStats.mean());
+    //out->printf("Std Dev: %f ", LFTStats.stdev());
     outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev();
-    out->printf("New outliers: %f, %f\n", outlierToF[0], outlierToF[1]);
+    //out->printf("New outliers: %f, %f\n", outlierToF[0], outlierToF[1]);