Where we will test the side ToF sensors
Dependencies: QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic
Diff: wheelchairControlSideTof/wheelchair.cpp
- Revision:
- 20:3c1b58654e67
- Parent:
- 18:a10277a63b53
- Child:
- 21:d1faccb96146
--- a/wheelchairControlSideTof/wheelchair.cpp Tue Jul 02 17:45:37 2019 +0000 +++ b/wheelchairControlSideTof/wheelchair.cpp Tue Jul 02 21:17:03 2019 +0000 @@ -139,10 +139,20 @@ //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; - out-> printf("Detecting wall!\n"); + if(sensor3 <= MIN_WALL_LENGTH) { + leftSafety = 1; + out-> printf("Detecting wall to the left!\n"); + } + else{ + leftSafety = 0; + } + + if(sensor6 <= MIN_WALL_LENGTH) { + rightSafety = 1; + out-> printf("Detecting wall to the right!\n"); + } + else { + rightSafety = 0; } //Check whether safe to keep turnin, user control <-- make sure @@ -154,9 +164,6 @@ // sideSafety = 1; //Not safe to turn //} //Safe to continue turning - else { - sideSafety = 0; - } /*-------Side Tof end -----------*/ } @@ -251,7 +258,7 @@ void Wheelchair::right() { //if safe to move, from ToFSafety - if(sideSafety == 0) { + if(rightSafety == 0) { out->printf("Moving right!\n"); x->write(def); y->write(low); @@ -262,7 +269,7 @@ void Wheelchair::left() { //if safe to move, from ToFSafety - if(sideSafety == 0) { + if(leftSafety == 0) { out->printf("Moving left!\n"); x->write(def); y->write(high);