Added more code for side and angled sensor
Dependencies: QEI2 PID Watchdog VL53L1X_Filter BNOWrapper ros_lib_kinetic
Diff: wheelchair.cpp
- Revision:
- 33:f3585571f11e
- Parent:
- 32:fb26baa75d44
- Child:
- 34:b89967adc86c
diff -r fb26baa75d44 -r f3585571f11e wheelchair.cpp --- a/wheelchair.cpp Mon Jul 01 23:35:39 2019 +0000 +++ b/wheelchair.cpp Tue Jul 02 16:40:41 2019 +0000 @@ -142,7 +142,7 @@ wheel = qei; ToF = ToFT; // passes pointer with addresses of ToF sensors - for(int i = 0; i < 12; i++) // initializes the ToF Sensors + for(int i = 0; i < 12; i++) // initializes the ToF Sensors { (*(ToF+i))->initReading(0x31+((0x02)*i), 50000); } @@ -162,7 +162,7 @@ //statistics LFTStats(ToFDataPointer1, 99, 1); -// //ToFDataPointer = ledgeArrayRF; + // //ToFDataPointer = ledgeArrayRF; //statistics RFTStats(ToFDataPointer2, 99, 1); @@ -170,8 +170,7 @@ 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("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 @@ -236,7 +235,7 @@ */ void Wheelchair::pid_right(int deg) { - bool overturn = false; //Boolean if angle over 360˚ + bool overturn = false; // Boolean if angle over 360˚ out->printf("pid right\r\r\n"); x->write(def); // Update x sent to chair to be stationary @@ -303,7 +302,7 @@ } myPID.SetTunings(5,0, 0.004); // Sets the constants for P and D - myPID.SetOutputLimits(0,high-def-.12); //Limit is set to the differnce between def and low + myPID.SetOutputLimits(0,high-def-.12); // Limit is set to the differnce between def and low myPID.SetControllerDirection(REVERSE); // PID mode: Reverse /* PID stops when approaching a litte more than desired angle */