Streams BNO055 IMU data
Dependencies: BNO055_fusion mbed NeoStrip
Dependents: MadPulse_Controller_ros
Fork of ES456_Labs by
main.cpp
- Committer:
- jdawkins
- Date:
- 2016-09-09
- Revision:
- 3:82e223a4a4e4
- Parent:
- 2:899128d20215
- Child:
- 5:b1c4a117eb4b
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