Jesi Miranda
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
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
--- /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
--- 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
--- 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);
}
--- 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
-
-
};