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
Diff: wheelchair.cpp
- Revision:
- 31:06f2362caf12
- Parent:
- 30:c25b2556e84d
- Child:
- 32:fb26baa75d44
--- a/wheelchair.cpp Mon Jul 01 21:38:57 2019 +0000 +++ b/wheelchair.cpp Mon Jul 01 23:20:48 2019 +0000 @@ -1,4 +1,3 @@ - #include "wheelchair.h" bool manual_drive = false; // Variable changes between joystick and auto drive @@ -12,12 +11,12 @@ // int* ToFDataPointer1; // int* ToFDataPointer2; -int ledgeArrayLF[100]; -int ledgeArrayRF[100]; +int ledgeArrayLF[150]; +int ledgeArrayRF[150]; int* ToFDataPointer1 = ledgeArrayLF; int* ToFDataPointer2 = ledgeArrayRF; -statistics LFTStats(ToFDataPointer1, 99, 1); -statistics RFTStats(ToFDataPointer1, 99, 1); +statistics LFTStats(ToFDataPointer1, 149, 1); +statistics RFTStats(ToFDataPointer2, 149, 1); int k = 0; double dist_old, curr_pos; // Variables for odometry position @@ -59,15 +58,24 @@ k++; - if (k == 100) { - k = 1; + if (k == 150) { + k = 0; } ledgeArrayLF[k] = (*(ToF+1))->readFromOneSensor(); ledgeArrayRF[k] = (*(ToF+4))->readFromOneSensor(); + /*for(int i = 0; i < 100; i++) + { + out->printf("%d, ",ledgeArrayRF[i]); + } + out->printf("\r\n");*/ // statistics LFTStats(ToFDataPointer1, 99, 1); // statistics RFTStats(ToFDataPointer1, 99, 1); + out->printf("Right Mean: %f ", RFTStats.mean()); + out->printf("Std Dev: % f", RFTStats.stdev()); outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev(); + out->printf("Left Mean: %f ", LFTStats.mean()); + out->printf("Std Dev: %f ", LFTStats.stdev()); outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev(); out->printf("New outliers: %f, %f\n", outlierToF[0], outlierToF[1]); @@ -107,12 +115,11 @@ 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 LEDGE\r\n"); } else forwardSafety = 0; - } /* Constructor for Wheelchair class */ @@ -142,27 +149,31 @@ out->printf("wheelchair setup done \r\n"); // Make sure it initialized; prints in serial monitor ti = time; - //(*(ToF+1))->readFromOneSensor(); -// (*(ToF+1))->readFromOneSensor(); - for(int i = 0; i < 100; i++) + for(int i = 0; i < 10; i++) + { + (*(ToF+1))->readFromOneSensor(); + (*(ToF+1))->readFromOneSensor(); + } + for(int i = 0; i < 150; i++) { ledgeArrayLF[i] = (*(ToF+1))->readFromOneSensor(); ledgeArrayRF[i] = (*(ToF+4))->readFromOneSensor(); } - statistics LFTStats(ToFDataPointer1, 99, 1); + //statistics LFTStats(ToFDataPointer1, 99, 1); // //ToFDataPointer = ledgeArrayRF; - statistics RFTStats(ToFDataPointer2, 99, 1); + //statistics RFTStats(ToFDataPointer2, 99, 1); + outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev(); outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev(); - out->printf("Left outlier = %f\n", outlierToF[0]); - out->printf("Right outlier = %f\n", outlierToF[1]); + //out->printf("Left outlier = %f\n", outlierToF[0]); + //out->printf("Right outlier = %f\n", outlierToF[1]); - out->printf("Left statistics = %f, %f\n", LFTStats.mean(), LFTStats.stdev()); - out->printf("Right statistics = %f, %f\n", RFTStats.mean(), RFTStats.stdev()); + //out->printf("Left statistics = %f, %f\n", LFTStats.mean(), LFTStats.stdev()); + //out->printf("Right statistics = %f, %f\n", RFTStats.mean(), RFTStats.stdev()); myPID.SetMode(AUTOMATIC); // PID mode: Automatic }