finished commenting on the .cpp file and changed the statistics library name

Dependencies:   QEI2 PID IMU6050Ver11 Watchdog VL53L1X_Filter ros_lib_kinetic

Dependents:   Version1-3 Version1-5

Files at this revision

API Documentation at this revision

Comitter:
JesiMiranda
Date:
Tue Jul 09 23:36:58 2019 +0000
Parent:
36:4131f0480f70
Commit message:
ff

Changed in this revision

IMU6050Ver11.lib Show annotated file Show diff for this revision Revisions of this file
chair_BNO055.lib Show diff for this revision Revisions of this file
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 4131f0480f70 -r e0e6d3fe06a2 IMU6050Ver11.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU6050Ver11.lib	Tue Jul 09 23:36:58 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/Affordable-Smart-Wheelchair/code/IMU6050Ver11/#f2e2692762ac
diff -r 4131f0480f70 -r e0e6d3fe06a2 chair_BNO055.lib
--- a/chair_BNO055.lib	Tue Jul 09 21:19:19 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/users/jvfausto/code/chair_BNO055/#ce8aa8208590
diff -r 4131f0480f70 -r e0e6d3fe06a2 wheelchair.cpp
--- a/wheelchair.cpp	Tue Jul 09 21:19:19 2019 +0000
+++ b/wheelchair.cpp	Tue Jul 09 23:36:58 2019 +0000
@@ -76,7 +76,7 @@
 }
 
 /*************************************************************************
-*      -------------------------------------------------------------     *
+*      Thread checks ToF sensors for safety of wheelchair movement       *
 **************************************************************************/
 void Wheelchair::ToFSafe_thread()
 {
@@ -140,46 +140,93 @@
     else
         forwardSafety = 0;
 
-    /*-------Side Tof begin----------*/
-
+    /*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;
+    double currAngularVelocity = imu->gyro_x(); //Current angular velocity from IMU
+    double angle = imu->yaw() * 3.14159 / 180; //from IMU, in rads
+    double arcLength = WheelchairRadius * currAngularVelocity * 
+                       currAngularVelocity / (2 * maxAngularDeceleration); //S = r*Ө, in cm
+   
+    /* 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 */
+        
+    //When either sensors too close to the wall, can't turn
+    if(sensor3 <= minWallLength) {
+        leftSafety = 1;
+        out-> printf("Detecting wall to the left!\n");
+    }
+    else{
+        leftSafety = 0;
+    }
+    
+    if(sensor6 <= minWallLength) {
+        rightSafety = 1;
+        out-> printf("Detecting wall to the right!\n");
+    }
+    else {
+        rightSafety = 0;
     }
     
-    /*************************************************************************
-    * 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;
+    /*Check whether safe to keep turning 
+    Know the exact moment you can stop the chair going at a certain speed before 
+    its too late*/
+    if((currAngularVelocity * currAngularVelocity > 2 * 
+        maxAngularDeceleration * angle) && (sensor3/10 <= arcLength + 10)) {
+        leftSafety = 1; //Not safe to turn left
+        out-> printf("Too fast to the left!\n");
+    }
+    else{
+        leftSafety = 0;
+       }
+    if((currAngularVelocity * currAngularVelocity > 2 * 
+        maxAngularDeceleration * angle) && (sensor6/10 <= arcLength + 10)) {
+        rightSafety = 1; //Not safe to turn right
+        out-> printf("Too fast to the right!\n");
     }
-
-    /*-------Side Tof end -----------*/
+    else{
+        rightSafety = 0;
+        }
+      
+    //Safe to continue turning 
+    //Check if can turn left and back side sensors
+    
+    //Check the back sensor
+    /*int sensor7 = ToFV[0]; //back sensor NOTTT SURE
+    int sensor8 = ToFV[3]; //back sensor
+    
+    if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor7 < curr_vel*curr_vel*1000*1000 || 
+    2 * maxDecelerationSlow*sensor8 < curr_vel*curr_vel*1000*1000) && 
+    (sensor7 < 1500 || sensor8 < 1500)) ||
+    550 > sensor7 || 550 > sensor8)
+    {
+        //out->printf("i am in danger\r\n");
+        if(x->read() > def)
+        {
+            x->write(def);
+            backwardSafety = 1;// You cannot move backward
+        }
+    }
+    //When going to fast to stop from wall
+    else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor7 < curr_vel*curr_vel*1000*1000 || 
+    2 * maxDecelerationFast*sensor8 < curr_vel*curr_vel*1000*1000) && 
+    (sensor7 < 1500 || sensor8 < 1500)) ||
+    550 > sensor7 || 550 > sensor8)
+    {
+        //out->printf("i am in danger\r\n");
+        if(x->read() > def)
+        {
+            x->write(def);
+            backwardSafety = 1;
+        }
+    }*/
+      
+    /*Side Tof end*/
+    
 }
 
 /*************************************************************************
@@ -199,7 +246,7 @@
     /* Initializes IMU Library */
     out = pc;                                                                           // "out" is called for serial monitor
     out->printf("on\r\n");
-    imu = new chair_BNO055(pc, time);
+    imu = new IMUWheelchair(pc, time);
     Wheelchair::stop();                                                                 // Wheelchair is initially stationary
     imu->setup();                                                                       // turns on the IMU
     wheelS = qeiS;                                                                      // "wheel" is called for encoder
@@ -269,7 +316,7 @@
 void Wheelchair::right()
 {
     //if safe to move, from ToFSafety
-    if(sideSafety == 0) {
+    if(rightSafety == 0) {
         x->write(def);
         y->write(low);
     }
@@ -280,7 +327,7 @@
 void Wheelchair::left()
 {
     //if safe to move, from ToFSafety
-    if(sideSafety == 0) {
+    if(leftSafety == 0) {
         x->write(def);
         y->write(high);
     }
diff -r 4131f0480f70 -r e0e6d3fe06a2 wheelchair.h
--- a/wheelchair.h	Tue Jul 09 21:19:19 2019 +0000
+++ b/wheelchair.h	Tue Jul 09 23:36:58 2019 +0000
@@ -3,7 +3,7 @@
 /*************************************************************************
 *            Importing libraries into wheelchair.h                       *
 **************************************************************************/
-#include "chair_BNO055.h"
+#include "IMUWheelchair.h"
 #include "PID.h"
 #include "QEI.h"
 #include "VL53L1X.h"
@@ -41,11 +41,11 @@
 /*************************************************************************
 *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 
+#define WheelchairRadius 80 //distance from IMU to edge of wheelchair(cm)
+#define  maxAngularDeceleration 1.04 //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)
+                                     //stop while turning. In rads per sec
+#define minWallLength 10 // minimum distance from wall to ToF (cm)
 /*************************************************************************
 *                                                                        *
 *                         Wheelchair class                               *
@@ -217,8 +217,10 @@
     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
+    bool backwardSafety;//Check if can move backward
+    bool leftSafety; //to check if can turn left
+    bool rightSafety; //to check if can turn right
+    double curr_yaw, curr_velS;// Variable that contains current relative angle
 
 private:
     /************************************************************************
@@ -238,14 +240,12 @@
 
     DigitalIn* e_button;                //Pointer to e_button
 
-    chair_BNO055* imu;                  // Pointer to IMU
+    IMUWheelchair* imu;                  // Pointer to IMU
     Serial* out;                        // Pointer to Serial Monitor
     Timer* ti;                          // Pointer to the timer
     QEI* wheel;                         // Pointer to encoder
     QEI* wheelS;                        // Pointer to encoder
     VL53L1X** ToF;                      // Arrays of pointers to ToF sensors
-    
-    
 
 
 };