Streams BNO055 IMU data
Dependencies: BNO055_fusion mbed NeoStrip
Dependents: MadPulse_Controller_ros
Fork of ES456_Labs by
Diff: main.cpp
- Revision:
- 0:42d1dda7d9c0
- Child:
- 2:899128d20215
diff -r 000000000000 -r 42d1dda7d9c0 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Jul 12 19:03:38 2016 +0000 @@ -0,0 +1,121 @@ +//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 + + +