Updated references from MPU6050 to BNO080
Dependencies: QEI2 PID Watchdog VL53L1X_Filter BNOWrapper ros_lib_kinetic
Diff: wheelchair.cpp
- Revision:
- 40:11b8567c507b
- Parent:
- 39:5438557a8b4e
- Child:
- 41:1a687bcf4b0a
diff -r 5438557a8b4e -r 11b8567c507b wheelchair.cpp --- a/wheelchair.cpp Wed Jul 10 19:30:57 2019 +0000 +++ b/wheelchair.cpp Thu Jul 11 18:38:03 2019 +0000 @@ -23,7 +23,7 @@ int* ToFDataPointer2 = ledgeArrayRF; Statistics LFTStats(ToFDataPointer1, 149, 1); Statistics RFTStats(ToFDataPointer2, 149, 1); -int k1 = 0; +int k = 0; int ledgeArrayLB[150]; int ledgeArrayRB[150]; @@ -31,7 +31,6 @@ 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]; @@ -96,14 +95,14 @@ //out->printf("\r\n"); - k1++; + k++; - if (k1 == 150) { - k1 = 0; + if (k == 150) { + k = 1; } - ledgeArrayLF[k1] = (*(ToF+1))->readFromOneSensor(); - ledgeArrayRF[k1] = (*(ToF+4))->readFromOneSensor(); + ledgeArrayLF[k] = (*(ToF+1))->readFromOneSensor(); + ledgeArrayRF[k] = (*(ToF+4))->readFromOneSensor(); /*for(int i = 0; i < 100; i++) { @@ -114,7 +113,7 @@ outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev(); outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev(); - for(int i = 0; i < 4; i++) { // Reads from the ToF Sensors + for(int i = 0; i < 2; i++) { // Reads from the ToF Sensors runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5); } @@ -150,19 +149,19 @@ //////////////////////////////////////////////////////////////////////////// - k2++; +// k2++; +// +// if (k2 == 150) { +// k2 = 0; +// } - if (k2 == 150) { - k2 = 0; - } - - ledgeArrayLB[k2] = (*(ToF+1))->readFromOneSensor(); - ledgeArrayRB[k2] = (*(ToF+4))->readFromOneSensor(); + ledgeArrayLB[k] = (*(ToF+1))->readFromOneSensor(); + ledgeArrayRB[k] = (*(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 + for(int i = 2; i < 4; i++) { // Reads from the ToF Sensors runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5); }