a

Dependencies:   QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic

Dependents:   wheelchairControlSumer19 Version1-0

Files at this revision

API Documentation at this revision

Comitter:
t1jain
Date:
Tue Jul 02 17:49:18 2019 +0000
Parent:
33:f3585571f11e
Commit message:
Cloned as Version 1.0

Changed in this revision

wheelchair.cpp Show annotated file Show diff for this revision Revisions of this file
wheelchair.h Show annotated file Show diff for this revision Revisions of this file
diff -r f3585571f11e -r b89967adc86c wheelchair.cpp
--- 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();
diff -r f3585571f11e -r b89967adc86c wheelchair.h
--- a/wheelchair.h	Tue Jul 02 16:40:41 2019 +0000
+++ b/wheelchair.h	Tue Jul 02 17:49:18 2019 +0000
@@ -36,6 +36,14 @@
 #define ToFSensorNum 12
 
 /*************************************************************************
+*IMU definitions for turning wheelchair
+**************************************************************************/
+#define WHEELCHAIR_RADIUS 56 //distance from IMU to edge of wheelchair(cm)
+#define  MAX_ANGULAR_DECELERATION 60 //found through testing, max 
+                                     //acceleration at which chair can 
+                                     //stop while turning. In degree per sec
+#define MIN_WALL_LENGTH 10 // minimum distance from wall to ToF (cm)
+/*************************************************************************
 *                                                                        *
 *                         Wheelchair class                               *
 *           Used for controlling the Smart Wheelchair                    *
@@ -206,6 +214,7 @@
     double vel;
     double test1, test2;
     bool forwardSafety;
+    bool sideSafety; //to check if can turn
     double curr_yaw, curr_velS;                                                            // Variable that contains current relative angle
 
 private: