Revision 37:e0e6d3fe06a2, committed 2019-07-09

Comitter:

JesiMiranda

Date:

Tue Jul 09 23:36:58 2019 +0000

Parent:

36:4131f0480f70

Commit message:

ff

Changed in this revision

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
-    
-    
 
 
 };