Added Back ToF code
Dependencies: QEI2 PID IMU6050Ver11 Watchdog VL53L1X_Filter ros_lib_kinetic
Dependents: Version1-6 Version1-7
Diff: wheelchair.cpp
- Revision:
- 27:da718b990837
- Parent:
- 26:662693bd7f31
- Child:
- 30:c25b2556e84d
diff -r 662693bd7f31 -r da718b990837 wheelchair.cpp --- a/wheelchair.cpp Thu Jun 27 16:32:12 2019 +0000 +++ b/wheelchair.cpp Fri Jun 28 21:16:26 2019 +0000 @@ -42,12 +42,19 @@ { ToFV[i] = (*(ToF+i))->readFromOneSensor(); //out->printf("%d ", ToFV[i]); - } + } + //out->printf("\r\n"); - int sensor1 = ToFV[1]; - int sensor4 = ToFV[5]; - //out->printf("%d, %d\r\n", sensor1, sensor4); - /*if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor1 < curr_vel*curr_vel*1000*1000 || + + + 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]); + 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)) || 550 > sensor1 || 550 > sensor4) @@ -73,8 +80,7 @@ } else forwardSafety = 0; - } - */ + } /* Constructor for Wheelchair class */ @@ -104,8 +110,14 @@ out->printf("wheelchair setup done \r\n"); // Make sure it initialized; prints in serial monitor ti = time; - - + for(int i = 0; i < 100; i++) + { + ledgeArrayLF[i] = (*(ToF+1))->readFromOneSensor(); + ledgeArrayRF[i] = (*(ToF+1))->readFromOneSensor(); + } + int* aaa = ledgeArrayLF; + statistics LFTStats(aaa, 100, 0); + out->printf("stadistics = %f, %f", LFTStats.mean(), LFTStats.stdev()); myPID.SetMode(AUTOMATIC); // PID mode: Automatic }