Diff: wheelchair.cpp

Revision:

38:cb87f4e353b9

Parent:

37:e0e6d3fe06a2

Child:

39:5438557a8b4e

--- a/wheelchair.cpp	Tue Jul 09 23:36:58 2019 +0000
+++ b/wheelchair.cpp	Wed Jul 10 18:06:58 2019 +0000
@@ -23,7 +23,15 @@
 int* ToFDataPointer2 = ledgeArrayRF;
 Statistics LFTStats(ToFDataPointer1, 149, 1);
 Statistics RFTStats(ToFDataPointer2, 149, 1);
-int k = 0;
+int k1 = 0;
+
+int ledgeArrayLB[150];
+int ledgeArrayRB[150];
+int* ToFDataPointer3 = ledgeArrayLB;
+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];
@@ -88,14 +96,14 @@
 
     //out->printf("\r\n");
 
-    k++;
+    k1++;
 
-    if (k == 150) {
-        k = 0;
+    if (k1 == 150) {
+        k1 = 0;
     }
 
-    ledgeArrayLF[k] = (*(ToF+1))->readFromOneSensor();
-    ledgeArrayRF[k] = (*(ToF+4))->readFromOneSensor();
+    ledgeArrayLF[k1] = (*(ToF+1))->readFromOneSensor();
+    ledgeArrayRF[k1] = (*(ToF+4))->readFromOneSensor();
 
     /*for(int i = 0; i < 100; i++)
     {
@@ -134,11 +142,62 @@
 
     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 FRONT LEDGE\r\n");
     }
 
     else
         forwardSafety = 0;
+    
+    ////////////////////////////////////////////////////////////////////////////
+    
+    k2++;
+
+    if (k2 == 150) {
+        k2 = 0;
+    }
+
+    ledgeArrayLB[k2] = (*(ToF+1))->readFromOneSensor();
+    ledgeArrayRB[k2] = (*(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
+        runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5);
+    }
+
+    int sensor7 = ToFV[6];
+    int sensor10 = ToFV[9];
+    if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor7 < curr_vel*curr_vel*1000*1000 ||
+                        2 * maxDecelerationSlow*sensor10 < curr_vel*curr_vel*1000*1000) &&
+                       (sensor7 < 1500 || sensor10 < 1500)) ||
+            550 > sensor7 || 550 > sensor10) {
+        if(x->read() > def) {
+            x->write(def);
+            backwardSafety = 1;          // You cannot move backward
+        }
+    }
+
+    else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor1 < curr_vel*curr_vel*1000*1000 ||
+                             2 * maxDecelerationFast*sensor4 < curr_vel*curr_vel*1000*1000) &&
+                            (sensor7 < 1500 || sensor10 < 1500)) ||
+            550 > sensor7 || 550 > sensor10) {
+        if(x->read() > def) {
+            x->write(def);
+            backwardSafety = 1;          // You cannot move backward
+        }
+    }
+
+    else if ((runningAverage[2] > outlierToF[2]) || (runningAverage[3] > outlierToF[3])) {
+        backwardSafety = 1;
+        out->printf("I'M STOPPING BECAUSE OF A BACK LEDGE\r\n");
+    }
+
+    else
+        backwardSafety = 0;
+    
+    
+    ////////////////////////////////////////////////////////////////////////////
 
     /*Side Tof begin*/
     int sensor3 = ToFV[2]; //front left