Diff: wheelchair.cpp

Revision:

40:11b8567c507b

Parent:

39:5438557a8b4e

Child:

41:1a687bcf4b0a

--- a/wheelchair.cpp	Wed Jul 10 19:30:57 2019 +0000
+++ b/wheelchair.cpp	Thu Jul 11 18:38:03 2019 +0000
@@ -23,7 +23,7 @@
 int* ToFDataPointer2 = ledgeArrayRF;
 Statistics LFTStats(ToFDataPointer1, 149, 1);
 Statistics RFTStats(ToFDataPointer2, 149, 1);
-int k1 = 0;
+int k = 0;
 
 int ledgeArrayLB[150];
 int ledgeArrayRB[150];
@@ -31,7 +31,6 @@
 int* ToFDataPointer4 = ledgeArrayRB;
 Statistics LBTStats(ToFDataPointer3, 149, 1);
 Statistics RBTStats(ToFDataPointer4, 149, 1);
-int k2 = 0;
 
 double dist_old, curr_pos;                                                             // Variables for odometry position
 double outlierToF[4];
@@ -96,14 +95,14 @@
 
     //out->printf("\r\n");
 
-    k1++;
+    k++;
 
-    if (k1 == 150) {
-        k1 = 0;
+    if (k == 150) {
+        k = 1;
     }
 
-    ledgeArrayLF[k1] = (*(ToF+1))->readFromOneSensor();
-    ledgeArrayRF[k1] = (*(ToF+4))->readFromOneSensor();
+    ledgeArrayLF[k] = (*(ToF+1))->readFromOneSensor();
+    ledgeArrayRF[k] = (*(ToF+4))->readFromOneSensor();
 
     /*for(int i = 0; i < 100; i++)
     {
@@ -114,7 +113,7 @@
     outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev();
     outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev();
 
-    for(int i = 0; i < 4; i++) {                             // Reads from the ToF Sensors
+    for(int i = 0; i < 2; i++) {                             // Reads from the ToF Sensors
         runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5);
     }
 
@@ -150,19 +149,19 @@
     
     ////////////////////////////////////////////////////////////////////////////
     
-    k2++;
+//        k2++;
+//    
+//        if (k2 == 150) {
+//            k2 = 0;
+//        }
 
-    if (k2 == 150) {
-        k2 = 0;
-    }
-
-    ledgeArrayLB[k2] = (*(ToF+1))->readFromOneSensor();
-    ledgeArrayRB[k2] = (*(ToF+4))->readFromOneSensor();
+    ledgeArrayLB[k] = (*(ToF+1))->readFromOneSensor();
+    ledgeArrayRB[k] = (*(ToF+4))->readFromOneSensor();
 
     outlierToF[2] = LBTStats.mean() + 2*LBTStats.stdev();
     outlierToF[3] = RBTStats.mean() + 2*RBTStats.stdev();
 
-    for(int i = 0; i < 4; i++) {                             // Reads from the ToF Sensors
+    for(int i = 2; i < 4; i++) {                             // Reads from the ToF Sensors
         runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5);
     }