Diff: wheelchair.cpp

Revision:

34:b89967adc86c

Parent:

33:f3585571f11e

--- a/wheelchair.cpp	Tue Jul 02 16:40:41 2019 +0000
+++ b/wheelchair.cpp	Tue Jul 02 17:49:18 2019 +0000
@@ -45,10 +45,28 @@
     curr_pos = wheel->getDistance(53.975);
 }
 
+void Wheelchair::emergencyButton_thread ()
+{
+    while(1) {
+        while(!e_button) {
+
+            //Stop wheelchair
+            Wheelchair::stop();
+            printf("E-button has been pressed\r\n");
+            off->write(high);                              // Turn off PCB
+            on->write(0);                                  // Make sure PCB not on
+            //Reset Board
+            NVIC_SystemReset();
+
+        }
+
+    }
+}
+
 void Wheelchair::ToFSafe_thread()
 {
     int ToFV[12];
-    for(int i = 0; i < 6; i++)                                              // reads from the ToF Sensors
+    for(int i = 0; i < 6; i++)                              // Reads from the ToF Sensors
     {
         ToFV[i] = (*(ToF+i))->readFromOneSensor();
         //out->printf("%d ", ToFV[i]);
@@ -64,22 +82,15 @@
     
     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]);
-        
-        
+    outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev();      
  
     for(int i = 0; i < 4; i++) {                             // Reads from the ToF Sensors
         runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5);
@@ -87,29 +98,27 @@
    
     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)
     {
-        //out->printf("i am in danger\r\n");
         if(x->read() > def)
         {
             x->write(def);
             forwardSafety = 1;          // You cannot move forward
         }
     }
+    
     else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor1 < curr_vel*curr_vel*1000*1000 || 
     2 * maxDecelerationFast*sensor4 < curr_vel*curr_vel*1000*1000) && 
     (sensor1 < 1500 || sensor4 < 1500)) ||
     550 > sensor1 || 550 > sensor4)
     {
-        //out->printf("i am in danger\r\n");
         if(x->read() > def)
         {
             x->write(def);
-            forwardSafety = 1;
+            forwardSafety = 1;          // You cannot move forward
         }
     }
     
@@ -120,6 +129,44 @@
     
     else
         forwardSafety = 0;
+        
+    /*-------Side Tof begin----------*/
+    
+    int sensor3 = ToFV[2]; //front left
+    int sensor6 = ToFV[5]; //front right
+    int sensor9 = ToFV[8]; //back
+    int sensor12 = ToFV[11]; //back
+    
+    // float currAngularVelocity = IMU DATA; //Current angular velocity from IMU
+    // float angle; //from IMU YAW, convert to cm
+    // float arcLength = angle * WHEELCHAIR_RADIUS; //S = r*Ө
+    
+    // Clear the front side first, else continue going straight or can't turn
+    // After clearing the front sideand movinf forward, check if can clear
+    // the back when turning
+    
+    // Check if can clear side
+    
+    // When either sensors too close to the wall, can't turn
+    if((sensor3 <= MIN_WALL_LENGTH) || (sensor6 <= MIN_WALL_LENGTH) ||
+       (sensor12 <= MIN_WALL_LENGTH)) {
+        sideSafety = 1;
+    }
+    
+    // Check whether safe to keep turnin, user control <-- make sure 
+    // currAngularVelocity is in correct units. Know the exact moment you can
+    // stop the chair going at a certain speed before its too late
+    // else if((currAngularVelocity * currAngularVelocity > 2 * 
+    //          MAX_ANGULAR_DECELERATION * angle) && (sensor3 <= angle ||
+    //          sensor6 <= angle)) {
+    //  sideSafety = 1; //Not safe to turn
+    // }
+    // Safe to continue turning
+    else {
+        sideSafety = 0;
+    }
+    
+    /*-------Side Tof end -----------*/
 }
 
 /* Constructor for Wheelchair class */
@@ -129,9 +176,11 @@
     x_position = 0;
     y_position = 0;
     forwardSafety = 0;
+    
     /* Initializes X and Y variables to Pins */
     x = new PwmOut(xPin);                                                               
     y = new PwmOut(yPin);
+    
     /* Initializes IMU Library */
     out = pc;                                                                           // "out" is called for serial monitor
     out->printf("on\r\n");
@@ -160,19 +209,9 @@
         ledgeArrayRF[i] = (*(ToF+4))->readFromOneSensor();
     }
     
-    
-    //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
 }
 
@@ -210,15 +249,21 @@
 /* Automatic mode: move right and update x,y coordinate sent to chair */
 void Wheelchair::right()                                                             
 {
-    x->write(def);
-    y->write(low);
+    //if safe to move, from ToFSafety
+    if(sideSafety == 0) {
+       x->write(def);
+       y->write(low);
+    }
 }
 
  /* Automatic mode: move left and update x,y coordinate sent to chair */
 void Wheelchair::left()                                                               
 {
-    x->write(def);
-    y->write(high);
+    //if safe to move, from ToFSafety
+    if(sideSafety == 0) {
+        x->write(def);
+        y->write(high);
+    }
 }
  
 /* Stop the wheelchair */
@@ -566,7 +611,7 @@
     return wheel->getDistance(Diameter);
 }
 
-/* This constructor resets the wheel encoder's */
+/* This constructor resets the wheel encoders */
 void Wheelchair::resetDistance()
 {
     wheel->reset();