Diff: wheelchair.cpp

Revision:

30:c25b2556e84d

Parent:

27:da718b990837

Child:

31:06f2362caf12

--- a/wheelchair.cpp	Mon Jul 01 16:36:47 2019 +0000
+++ b/wheelchair.cpp	Mon Jul 01 21:38:57 2019 +0000
@@ -1,6 +1,6 @@
 
 #include "wheelchair.h"
- 
+
 bool manual_drive = false;                                                             // Variable changes between joystick and auto drive
 double encoder_distance;                                                               // Keeps distanse due to original position
  
@@ -9,8 +9,19 @@
 volatile double vIn, vOut, vDesired;                                                   // Variables for PID Velosity
 volatile double vInS, vOutS, vDesiredS;                                                // Variables for PID Slave Wheel
 volatile double yIn, yOut, yDesired;                                                   // Variables for PID turn velosity
+//    int* ToFDataPointer1;
+//    int* ToFDataPointer2;
+
+int ledgeArrayLF[100];            
+int ledgeArrayRF[100]; 
+int* ToFDataPointer1 = ledgeArrayLF;
+int* ToFDataPointer2 = ledgeArrayRF;
+statistics LFTStats(ToFDataPointer1, 99, 1);
+statistics RFTStats(ToFDataPointer1, 99, 1); 
+int k = 0;
 
 double dist_old, curr_pos;                                                             // Variables for odometry position
+double outlierToF[4];
 
  
 PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT);             // Angle PID object constructor
@@ -35,25 +46,40 @@
     curr_pos = wheel->getDistance(53.975);
 }
 
-void Wheelchair::assistSafe_thread()
+void Wheelchair::ToFSafe_thread()
 {
     int ToFV[12];
     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++;
+    
+    if (k == 100) {
+        k = 1;
+    }
     
+    ledgeArrayLF[k] = (*(ToF+1))->readFromOneSensor();
+    ledgeArrayRF[k] = (*(ToF+4))->readFromOneSensor();
+//    statistics LFTStats(ToFDataPointer1, 99, 1);
+//    statistics RFTStats(ToFDataPointer1, 99, 1);  
+    outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev();
+    outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev();
+    out->printf("New outliers: %f, %f\n", outlierToF[0], outlierToF[1]);
+        
+        
  
     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]);
+    //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)) ||
@@ -63,7 +89,7 @@
         if(x->read() > def)
         {
             x->write(def);
-            forwardSafety = 1;
+            forwardSafety = 1;          // You cannot move forward
         }
     }
     else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor1 < curr_vel*curr_vel*1000*1000 || 
@@ -78,6 +104,12 @@
             forwardSafety = 1;
         }
     }
+    
+    else if ((runningAverage[0] > outlierToF[0]) || (runningAverage[1] > outlierToF[1])) {
+        forwardSafety = 1;
+        out->printf("I'm stopping because of a ledge\r\n");
+        }
+    
     else
         forwardSafety = 0;
 
@@ -110,14 +142,27 @@
          
     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++)
     {
         ledgeArrayLF[i] = (*(ToF+1))->readFromOneSensor();
-        ledgeArrayRF[i] = (*(ToF+1))->readFromOneSensor();
+        ledgeArrayRF[i] = (*(ToF+4))->readFromOneSensor();
     }
-    int* aaa = ledgeArrayLF; 
-    statistics LFTStats(aaa, 100, 0); 
-    out->printf("stadistics = %f, %f",  LFTStats.mean(), LFTStats.stdev());
+    
+    
+    statistics LFTStats(ToFDataPointer1, 99, 1); 
+//    //ToFDataPointer = ledgeArrayRF;
+    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 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
 }