Updated references from MPU6050 to BNO080
Dependencies: QEI2 PID Watchdog VL53L1X_Filter BNOWrapper ros_lib_kinetic
Diff: wheelchair.cpp
- Revision:
- 38:cb87f4e353b9
- Parent:
- 37:e0e6d3fe06a2
- Child:
- 39:5438557a8b4e
diff -r e0e6d3fe06a2 -r cb87f4e353b9 wheelchair.cpp --- a/wheelchair.cpp Tue Jul 09 23:36:58 2019 +0000 +++ b/wheelchair.cpp Wed Jul 10 18:06:58 2019 +0000 @@ -23,7 +23,15 @@ int* ToFDataPointer2 = ledgeArrayRF; Statistics LFTStats(ToFDataPointer1, 149, 1); Statistics RFTStats(ToFDataPointer2, 149, 1); -int k = 0; +int k1 = 0; + +int ledgeArrayLB[150]; +int ledgeArrayRB[150]; +int* ToFDataPointer3 = ledgeArrayLB; +int* ToFDataPointer4 = ledgeArrayRB; +Statistics LBTStats(ToFDataPointer3, 149, 1); +Statistics RBTStats(ToFDataPointer4, 149, 1); +int k2 = 0; double dist_old, curr_pos; // Variables for odometry position double outlierToF[4]; @@ -88,14 +96,14 @@ //out->printf("\r\n"); - k++; + k1++; - if (k == 150) { - k = 0; + if (k1 == 150) { + k1 = 0; } - ledgeArrayLF[k] = (*(ToF+1))->readFromOneSensor(); - ledgeArrayRF[k] = (*(ToF+4))->readFromOneSensor(); + ledgeArrayLF[k1] = (*(ToF+1))->readFromOneSensor(); + ledgeArrayRF[k1] = (*(ToF+4))->readFromOneSensor(); /*for(int i = 0; i < 100; i++) { @@ -134,11 +142,62 @@ else if ((runningAverage[0] > outlierToF[0]) || (runningAverage[1] > outlierToF[1])) { forwardSafety = 1; - out->printf("I'M STOPPING BECAUSE OF A LEDGE\r\n"); + out->printf("I'M STOPPING BECAUSE OF A FRONT LEDGE\r\n"); } else forwardSafety = 0; + + //////////////////////////////////////////////////////////////////////////// + + k2++; + + if (k2 == 150) { + k2 = 0; + } + + ledgeArrayLB[k2] = (*(ToF+1))->readFromOneSensor(); + ledgeArrayRB[k2] = (*(ToF+4))->readFromOneSensor(); + + outlierToF[2] = LBTStats.mean() + 2*LBTStats.stdev(); + outlierToF[3] = RBTStats.mean() + 2*RBTStats.stdev(); + + for(int i = 0; i < 4; i++) { // Reads from the ToF Sensors + runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5); + } + + int sensor7 = ToFV[6]; + int sensor10 = ToFV[9]; + if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor7 < curr_vel*curr_vel*1000*1000 || + 2 * maxDecelerationSlow*sensor10 < curr_vel*curr_vel*1000*1000) && + (sensor7 < 1500 || sensor10 < 1500)) || + 550 > sensor7 || 550 > sensor10) { + if(x->read() > def) { + x->write(def); + backwardSafety = 1; // You cannot move backward + } + } + + else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor1 < curr_vel*curr_vel*1000*1000 || + 2 * maxDecelerationFast*sensor4 < curr_vel*curr_vel*1000*1000) && + (sensor7 < 1500 || sensor10 < 1500)) || + 550 > sensor7 || 550 > sensor10) { + if(x->read() > def) { + x->write(def); + backwardSafety = 1; // You cannot move backward + } + } + + else if ((runningAverage[2] > outlierToF[2]) || (runningAverage[3] > outlierToF[3])) { + backwardSafety = 1; + out->printf("I'M STOPPING BECAUSE OF A BACK LEDGE\r\n"); + } + + else + backwardSafety = 0; + + + //////////////////////////////////////////////////////////////////////////// /*Side Tof begin*/ int sensor3 = ToFV[2]; //front left