Diff: wheelchair.cpp

Revision:

31:06f2362caf12

Parent:

30:c25b2556e84d

Child:

32:fb26baa75d44

--- a/wheelchair.cpp	Mon Jul 01 21:38:57 2019 +0000
+++ b/wheelchair.cpp	Mon Jul 01 23:20:48 2019 +0000
@@ -1,4 +1,3 @@
-
 #include "wheelchair.h"
 
 bool manual_drive = false;                                                             // Variable changes between joystick and auto drive
@@ -12,12 +11,12 @@
 //    int* ToFDataPointer1;
 //    int* ToFDataPointer2;
 
-int ledgeArrayLF[100];            
-int ledgeArrayRF[100]; 
+int ledgeArrayLF[150];            
+int ledgeArrayRF[150]; 
 int* ToFDataPointer1 = ledgeArrayLF;
 int* ToFDataPointer2 = ledgeArrayRF;
-statistics LFTStats(ToFDataPointer1, 99, 1);
-statistics RFTStats(ToFDataPointer1, 99, 1); 
+statistics LFTStats(ToFDataPointer1, 149, 1);
+statistics RFTStats(ToFDataPointer2, 149, 1); 
 int k = 0;
 
 double dist_old, curr_pos;                                                             // Variables for odometry position
@@ -59,15 +58,24 @@
     
     k++;
     
-    if (k == 100) {
-        k = 1;
+    if (k == 150) {
+        k = 0;
     }
     
     ledgeArrayLF[k] = (*(ToF+1))->readFromOneSensor();
     ledgeArrayRF[k] = (*(ToF+4))->readFromOneSensor();
+    /*for(int i = 0; i < 100; i++)
+    {
+        out->printf("%d, ",ledgeArrayRF[i]);
+    }
+    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());
     outlierToF[0] = LFTStats.mean() + 2*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]);
         
@@ -107,12 +115,11 @@
     
     else if ((runningAverage[0] > outlierToF[0]) || (runningAverage[1] > outlierToF[1])) {
         forwardSafety = 1;
-        out->printf("I'm stopping because of a ledge\r\n");
+        out->printf("I'M STOPPING BECAUSE OF A LEDGE\r\n");
         }
     
     else
         forwardSafety = 0;
-
 }
 
 /* Constructor for Wheelchair class */
@@ -142,27 +149,31 @@
          
     out->printf("wheelchair setup done \r\n");                                          // Make sure it initialized; prints in serial monitor
     ti = time;
-    //(*(ToF+1))->readFromOneSensor();
-//    (*(ToF+1))->readFromOneSensor();
-    for(int i = 0; i < 100; i++)
+    for(int i = 0; i < 10; i++)
+    {
+    (*(ToF+1))->readFromOneSensor();
+    (*(ToF+1))->readFromOneSensor();
+    }
+    for(int i = 0; i < 150; i++)
     {
         ledgeArrayLF[i] = (*(ToF+1))->readFromOneSensor();
         ledgeArrayRF[i] = (*(ToF+4))->readFromOneSensor();
     }
     
     
-    statistics LFTStats(ToFDataPointer1, 99, 1); 
+    //statistics LFTStats(ToFDataPointer1, 99, 1); 
 //    //ToFDataPointer = ledgeArrayRF;
-    statistics RFTStats(ToFDataPointer2, 99, 1); 
+    //statistics RFTStats(ToFDataPointer2, 99, 1); 
+    
     
     outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev();
     outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev();
     
-    out->printf("Left outlier = %f\n",  outlierToF[0]);
-    out->printf("Right outlier = %f\n",  outlierToF[1]);
+    //out->printf("Left outlier = %f\n",  outlierToF[0]);
+    //out->printf("Right outlier = %f\n",  outlierToF[1]);
     
-    out->printf("Left statistics = %f, %f\n",  LFTStats.mean(), LFTStats.stdev());
-    out->printf("Right statistics = %f, %f\n",  RFTStats.mean(), RFTStats.stdev());
+    //out->printf("Left statistics = %f, %f\n",  LFTStats.mean(), LFTStats.stdev());
+    //out->printf("Right statistics = %f, %f\n",  RFTStats.mean(), RFTStats.stdev());
     myPID.SetMode(AUTOMATIC);                                                           // PID mode: Automatic
 }