File content as of revision 3:82e223a4a4e4:

//Uses the measured z-acceleration to drive leds 2 and 3 of the mbed

#define LOG_RATE 50.0
#define LOOP_RATE 200.0
#define CMD_TIMEOUT 1.0
#define GEAR_RATIO (1/2.75)
#define PI 3.14159
#include "mbed.h"

#include "BNO055.h"


BNO055 imu(p9, p10);


int left;
float saturateCmd(float cmd);
void menuFunction(Serial *port);
DigitalOut status_LED(LED1);
DigitalOut armed_LED(LED2);
DigitalOut auto_LED(LED3);
DigitalOut imu_LED(LED4);

Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
Serial xbee(p28, p27); // tx, rx for Xbee




Timer t; // create timer instance

float t_imu,t_cmd,str_cmd,thr_cmd,str,thr;
float t_hall, dt_hall,t_run,t_stop,t_log;
bool armed, auto_ctrl;
float wheel_spd;
float arm_clock;
bool str_cond,thr_cond,run_ctrl,log_data;
bool log_imu,log_bno,log_odo,log_mag = false;

int main()
{

    pc.baud(115200);
    xbee.baud(115200);

//imu.init_MPU_i2c();
    left = 0;
    str_cmd = 0;
    str=0;
    thr=0;
    thr_cmd = 0;
    arm_clock =0;
    str_cond = false;
    thr_cond = false;
    armed = false;
    auto_ctrl = false;
    run_ctrl = false;
    log_data = false;

    t.start();
    t_imu = t.read();
    t_cmd = 0;

    status_LED = 1;

    if(imu.check()) {
        
        pc.printf("BNO055 connected\r\n");
        imu.setmode(OPERATION_MODE_CONFIG);
        imu.SetExternalCrystal(1);
        imu.setmode(OPERATION_MODE_NDOF);  //Uses magnetometer
        imu.set_angle_units(DEGREES);
        imu.set_accel_units(MPERSPERS);
        imu.set_anglerate_units(DEG_PER_SEC);        
        imu.setoutputformat(WINDOWS);
        imu.set_mapping(2);

        
            
    } else {
        pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
        status_LED = 1;
        armed_LED = 1;
        imu_LED = 1;
        auto_LED = 1;
        while(1) {
            status_LED = !status_LED;
            armed_LED = !armed_LED;
            imu_LED = !imu_LED;
            auto_LED = !auto_LED;
            wait(0.5);
        }
    }

    pc.printf("ES456 Vehicle Sensor Logger\r\n");
    while(1){
                         
        imu.get_angles();
        imu.get_accel();
        imu.get_gyro();
        imu.get_mag();
         if(t.read()-t_imu > (1/LOG_RATE)) {


             //pc.printf("$MAD,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.1f,%.1f,%.1f\r\n", imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.accel.x,imu.accel.y,imu.accel.z,imu.mag.x,imu.mag.y,imu.mag.z);             
             //xbee.printf("$MAD,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.1f,%.1f,%.1f\r\n", imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.accel.x,imu.accel.y,imu.accel.z,imu.mag.x,imu.mag.y,imu.mag.z);

             pc.printf("$IMU,%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", imu.accel.x,imu.accel.y,imu.accel.z,imu.gyro.x,imu.gyro.y,imu.gyro.z);             
             pc.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);
             
           //  xbee.printf("$IMU,%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", imu.accel.x,imu.accel.y,imu.accel.z,imu.gyro.x,imu.gyro.y,imu.gyro.z);             
           //  xbee.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);
             
             t_imu = t.read();
         } // if t.read
        wait(1/LOOP_RATE);
        status_LED=!status_LED;
    } // while (1)

} // main