Added Back ToF code
Dependencies: QEI2 PID IMU6050Ver11 Watchdog VL53L1X_Filter ros_lib_kinetic
Dependents: Version1-6 Version1-7
Diff: wheelchair.cpp
- Revision:
- 30:c25b2556e84d
- Parent:
- 27:da718b990837
- Child:
- 31:06f2362caf12
diff -r 4519f4cdcb5d -r c25b2556e84d wheelchair.cpp --- a/wheelchair.cpp Mon Jul 01 16:36:47 2019 +0000 +++ b/wheelchair.cpp Mon Jul 01 21:38:57 2019 +0000 @@ -1,6 +1,6 @@ #include "wheelchair.h" - + bool manual_drive = false; // Variable changes between joystick and auto drive double encoder_distance; // Keeps distanse due to original position @@ -9,8 +9,19 @@ volatile double vIn, vOut, vDesired; // Variables for PID Velosity volatile double vInS, vOutS, vDesiredS; // Variables for PID Slave Wheel volatile double yIn, yOut, yDesired; // Variables for PID turn velosity +// int* ToFDataPointer1; +// int* ToFDataPointer2; + +int ledgeArrayLF[100]; +int ledgeArrayRF[100]; +int* ToFDataPointer1 = ledgeArrayLF; +int* ToFDataPointer2 = ledgeArrayRF; +statistics LFTStats(ToFDataPointer1, 99, 1); +statistics RFTStats(ToFDataPointer1, 99, 1); +int k = 0; double dist_old, curr_pos; // Variables for odometry position +double outlierToF[4]; PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT); // Angle PID object constructor @@ -35,25 +46,40 @@ curr_pos = wheel->getDistance(53.975); } -void Wheelchair::assistSafe_thread() +void Wheelchair::ToFSafe_thread() { int ToFV[12]; for(int i = 0; i < 6; i++) // reads from the ToF Sensors { ToFV[i] = (*(ToF+i))->readFromOneSensor(); - //out->printf("%d ", ToFV[i]); + out->printf("%d ", ToFV[i]); } - //out->printf("\r\n"); + out->printf("\r\n"); + + k++; + + if (k == 100) { + k = 1; + } + ledgeArrayLF[k] = (*(ToF+1))->readFromOneSensor(); + ledgeArrayRF[k] = (*(ToF+4))->readFromOneSensor(); +// statistics LFTStats(ToFDataPointer1, 99, 1); +// statistics RFTStats(ToFDataPointer1, 99, 1); + outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev(); + outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev(); + out->printf("New outliers: %f, %f\n", outlierToF[0], outlierToF[1]); + + for(int i = 0; i < 4; i++) { // Reads from the ToF Sensors runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5); - } + } int sensor1 = ToFV[0]; int sensor4 = ToFV[3]; - out->printf("%d, %d\r\n", ToFV[1], runningAverage[0]); + //out->printf("%d, %d\r\n", ToFV[1], runningAverage[0]); if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor1 < curr_vel*curr_vel*1000*1000 || 2 * maxDecelerationSlow*sensor4 < curr_vel*curr_vel*1000*1000) && (sensor1 < 1500 || sensor4 < 1500)) || @@ -63,7 +89,7 @@ if(x->read() > def) { x->write(def); - forwardSafety = 1; + forwardSafety = 1; // You cannot move forward } } else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor1 < curr_vel*curr_vel*1000*1000 || @@ -78,6 +104,12 @@ forwardSafety = 1; } } + + else if ((runningAverage[0] > outlierToF[0]) || (runningAverage[1] > outlierToF[1])) { + forwardSafety = 1; + out->printf("I'm stopping because of a ledge\r\n"); + } + else forwardSafety = 0; @@ -110,14 +142,27 @@ 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++) { ledgeArrayLF[i] = (*(ToF+1))->readFromOneSensor(); - ledgeArrayRF[i] = (*(ToF+1))->readFromOneSensor(); + ledgeArrayRF[i] = (*(ToF+4))->readFromOneSensor(); } - int* aaa = ledgeArrayLF; - statistics LFTStats(aaa, 100, 0); - out->printf("stadistics = %f, %f", LFTStats.mean(), LFTStats.stdev()); + + + statistics LFTStats(ToFDataPointer1, 99, 1); +// //ToFDataPointer = ledgeArrayRF; + 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 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 }