Added more code for side and angled sensor

Dependencies:   QEI2 PID Watchdog VL53L1X_Filter BNOWrapper ros_lib_kinetic

Revision:
43:9858a580b3d7
Parent:
21:3489cffad196