Added more code for side and angled sensor

Dependencies:   QEI2 PID Watchdog VL53L1X_Filter BNOWrapper ros_lib_kinetic

Revision:
35:5a2fed4c2e9f
Parent:
34:b89967adc86c
Child:
37:e0e6d3fe06a2
--- a/wheelchair.h	Tue Jul 02 17:49:18 2019 +0000
+++ b/wheelchair.h	Tue Jul 09 21:18:01 2019 +0000
@@ -7,12 +7,14 @@
 #include "PID.h"
 #include "QEI.h"
 #include "VL53L1X.h"
-#include "statistics.h"
+#include "Statistics.h"
 #include "Watchdog.h"
 
 #include <ros.h>
 #include <geometry_msgs/Twist.h>
 #include <nav_msgs/Odometry.h>
+#include <std_msgs/Float64.h>
+#include <std_msgs/Float64MultiArray.h>
 
 /*************************************************************************
 * Joystick has analog out of 200-700, scale values between 1.3 and 3.3   *
@@ -23,6 +25,7 @@
 #define low (1.7f/3.3f)                 // Low power on joystick; used on x and y axis
 #define offset .03f                     // Joystick adjustment to be able to go straight. Chair dependent on manufactoring precision
 #define process .1                      // Defines default time delay in seconds
+
 /*************************************************************************
 *Pin plug-in for Nucleo-L432KC/Compatible with Nucleo-400 series (F767ZI)*
 **************************************************************************/
@@ -218,12 +221,12 @@
     double curr_yaw, curr_velS;                                                            // Variable that contains current relative angle
 
 private:
+    /************************************************************************
+     * Expected data used to compare whether of not there is a ledge.       *
+     * This serves as a ground base. Array is used for calibrating the      *
+     * time of flight sensors, which is used to calculate stdev and mean on * 
+     ************************************************************************/
     int runningAverage[4];           
-    // Expected data used to compare whether of not there is a ledge. This serves as a ground base
-    // Array is used for calibrating the time of flight sensors  
-    // Used to calculate stdev and mean on 
-
-
 
     /* Pointers for the joystick speed */
     PwmOut* x;