File content as of revision 1:d56e1d85e6d8:

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

#define LOG_RATE 25.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 hall_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);
        //bno.set_orientation(1);
        imu.setmode(OPERATION_MODE_NDOF);  //Uses magnetometer
        //bno.setmode(OPERATION_MODE_NDOF_FMC_OFF);   //no magnetometer
        imu.set_angle_units(DEGREES);
        imu.set_mapping(1);
    } else {
        pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
        status_LED = 1;
        armed_LED = 1;
        hall_LED = 1;
        auto_LED = 1;
        while(1) {
            status_LED = !status_LED;
            armed_LED = !armed_LED;
            hall_LED = !hall_LED;
            auto_LED = !auto_LED;
            wait(0.5);
        }
    }

    pc.printf("ES456 Vehcile Program\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("%.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("$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);
             pc.printf("$BNO,%.2f, %.2f, %.2f\r\n",imu.euler.roll,imu.euler.pitch,imu.euler.yaw);
             
             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);
             xbee.printf("$BNO,%.2f, %.2f, %.2f\r\n",imu.euler.roll,imu.euler.pitch,imu.euler.yaw);             
             
             xbee.printf("%.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);

             t_imu = t.read();
         } // if t.read
        wait(1/LOOP_RATE);
        status_LED=!status_LED;
    } // while (1)

} // main