Diff: wheelchair.cpp

Revision:

27:da718b990837

Parent:

26:662693bd7f31

Child:

30:c25b2556e84d

--- a/wheelchair.cpp	Thu Jun 27 16:32:12 2019 +0000
+++ b/wheelchair.cpp	Fri Jun 28 21:16:26 2019 +0000
@@ -42,12 +42,19 @@
     {
         ToFV[i] = (*(ToF+i))->readFromOneSensor();
         //out->printf("%d ", ToFV[i]);
-    }        
+    }  
+  
         //out->printf("\r\n");
-    int sensor1 = ToFV[1];
-    int sensor4 = ToFV[5];
-    //out->printf("%d, %d\r\n", sensor1, sensor4);
-    /*if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor1 < curr_vel*curr_vel*1000*1000 || 
+    
+ 
+    for(int i = 0; i < 4; i++) {                             // Reads from the ToF Sensors
+        runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5);
+    }    
+   
+    int sensor1 = ToFV[0];
+    int sensor4 = ToFV[3];
+    out->printf("%d, %d\r\n", ToFV[1], runningAverage[0]);
+    if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor1 < curr_vel*curr_vel*1000*1000 || 
     2 * maxDecelerationSlow*sensor4 < curr_vel*curr_vel*1000*1000) && 
     (sensor1 < 1500 || sensor4 < 1500)) ||
     550 > sensor1 || 550 > sensor4)
@@ -73,8 +80,7 @@
     }
     else
         forwardSafety = 0;
-    }
-    */
+
 }
 
 /* Constructor for Wheelchair class */
@@ -104,8 +110,14 @@
          
     out->printf("wheelchair setup done \r\n");                                          // Make sure it initialized; prints in serial monitor
     ti = time;
-    
-
+    for(int i = 0; i < 100; i++)
+    {
+        ledgeArrayLF[i] = (*(ToF+1))->readFromOneSensor();
+        ledgeArrayRF[i] = (*(ToF+1))->readFromOneSensor();
+    }
+    int* aaa = ledgeArrayLF; 
+    statistics LFTStats(aaa, 100, 0); 
+    out->printf("stadistics = %f, %f",  LFTStats.mean(), LFTStats.stdev());
     myPID.SetMode(AUTOMATIC);                                                           // PID mode: Automatic
 }