Waypoint Command + Obstacle Avoidance + Controller

Dependencies:   mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler MAX17048 Servo MODSERIAL

Fork of Orion_PCB_test_Faulhaber_gear_ratio41_waypoint_cmd by Team Virgo v3