Added Back ToF code
Dependencies: QEI2 PID IMU6050Ver11 Watchdog VL53L1X_Filter ros_lib_kinetic
Dependents: Version1-6 Version1-7
Diff: wheelchair.cpp
- Revision:
- 41:1a687bcf4b0a
- Parent:
- 40:11b8567c507b
diff -r 11b8567c507b -r 1a687bcf4b0a wheelchair.cpp --- a/wheelchair.cpp Thu Jul 11 18:38:03 2019 +0000 +++ b/wheelchair.cpp Thu Jul 11 20:58:26 2019 +0000 @@ -104,11 +104,11 @@ ledgeArrayLF[k] = (*(ToF+1))->readFromOneSensor(); ledgeArrayRF[k] = (*(ToF+4))->readFromOneSensor(); - /*for(int i = 0; i < 100; i++) + /* for(int i = 0; i < 100; i++) { out->printf("%d, ",ledgeArrayRF[i]); } - out->printf("\r\n");*/ + out->printf("\r\n"); */ outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev(); outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev(); @@ -148,12 +148,6 @@ forwardSafety = 0; //////////////////////////////////////////////////////////////////////////// - -// k2++; -// -// if (k2 == 150) { -// k2 = 0; -// } ledgeArrayLB[k] = (*(ToF+1))->readFromOneSensor(); ledgeArrayRB[k] = (*(ToF+4))->readFromOneSensor(); @@ -194,8 +188,7 @@ else backwardSafety = 0; - - + //////////////////////////////////////////////////////////////////////////// /*Side Tof begin*/