Added more code for side and angled sensor

Dependencies:   QEI2 PID Watchdog VL53L1X_Filter BNOWrapper ros_lib_kinetic

Revision:
33:f3585571f11e
Parent:
32:fb26baa75d44
Child:
34:b89967adc86c
--- 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 */