Streams BNO055 IMU data
Dependencies: BNO055_fusion mbed NeoStrip
Dependents: MadPulse_Controller_ros
Fork of ES456_Labs by
main.cpp@3:82e223a4a4e4, 2016-09-09 (annotated)
- Committer:
- jdawkins
- Date:
- Fri Sep 09 15:28:02 2016 +0000
- Revision:
- 3:82e223a4a4e4
- Parent:
- 2:899128d20215
- Child:
- 5:b1c4a117eb4b
Modified print statements to output $IMU and $MAG messages. This will work with Matlab madPulseCar Class.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jdawkins | 0:42d1dda7d9c0 | 1 | //Uses the measured z-acceleration to drive leds 2 and 3 of the mbed |
jdawkins | 0:42d1dda7d9c0 | 2 | |
jdawkins | 3:82e223a4a4e4 | 3 | #define LOG_RATE 50.0 |
jdawkins | 0:42d1dda7d9c0 | 4 | #define LOOP_RATE 200.0 |
jdawkins | 0:42d1dda7d9c0 | 5 | #define CMD_TIMEOUT 1.0 |
jdawkins | 0:42d1dda7d9c0 | 6 | #define GEAR_RATIO (1/2.75) |
jdawkins | 0:42d1dda7d9c0 | 7 | #define PI 3.14159 |
jdawkins | 0:42d1dda7d9c0 | 8 | #include "mbed.h" |
jdawkins | 0:42d1dda7d9c0 | 9 | |
jdawkins | 0:42d1dda7d9c0 | 10 | #include "BNO055.h" |
jdawkins | 0:42d1dda7d9c0 | 11 | |
jdawkins | 0:42d1dda7d9c0 | 12 | |
jdawkins | 0:42d1dda7d9c0 | 13 | BNO055 imu(p9, p10); |
jdawkins | 0:42d1dda7d9c0 | 14 | |
jdawkins | 0:42d1dda7d9c0 | 15 | |
jdawkins | 0:42d1dda7d9c0 | 16 | int left; |
jdawkins | 0:42d1dda7d9c0 | 17 | float saturateCmd(float cmd); |
jdawkins | 0:42d1dda7d9c0 | 18 | void menuFunction(Serial *port); |
jdawkins | 0:42d1dda7d9c0 | 19 | DigitalOut status_LED(LED1); |
jdawkins | 0:42d1dda7d9c0 | 20 | DigitalOut armed_LED(LED2); |
jdawkins | 0:42d1dda7d9c0 | 21 | DigitalOut auto_LED(LED3); |
jdawkins | 2:899128d20215 | 22 | DigitalOut imu_LED(LED4); |
jdawkins | 0:42d1dda7d9c0 | 23 | |
jdawkins | 0:42d1dda7d9c0 | 24 | Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc |
jdawkins | 0:42d1dda7d9c0 | 25 | Serial xbee(p28, p27); // tx, rx for Xbee |
jdawkins | 0:42d1dda7d9c0 | 26 | |
jdawkins | 0:42d1dda7d9c0 | 27 | |
jdawkins | 0:42d1dda7d9c0 | 28 | |
jdawkins | 0:42d1dda7d9c0 | 29 | |
jdawkins | 0:42d1dda7d9c0 | 30 | Timer t; // create timer instance |
jdawkins | 0:42d1dda7d9c0 | 31 | |
jdawkins | 0:42d1dda7d9c0 | 32 | float t_imu,t_cmd,str_cmd,thr_cmd,str,thr; |
jdawkins | 0:42d1dda7d9c0 | 33 | float t_hall, dt_hall,t_run,t_stop,t_log; |
jdawkins | 0:42d1dda7d9c0 | 34 | bool armed, auto_ctrl; |
jdawkins | 0:42d1dda7d9c0 | 35 | float wheel_spd; |
jdawkins | 0:42d1dda7d9c0 | 36 | float arm_clock; |
jdawkins | 0:42d1dda7d9c0 | 37 | bool str_cond,thr_cond,run_ctrl,log_data; |
jdawkins | 0:42d1dda7d9c0 | 38 | bool log_imu,log_bno,log_odo,log_mag = false; |
jdawkins | 0:42d1dda7d9c0 | 39 | |
jdawkins | 0:42d1dda7d9c0 | 40 | int main() |
jdawkins | 0:42d1dda7d9c0 | 41 | { |
jdawkins | 0:42d1dda7d9c0 | 42 | |
jdawkins | 0:42d1dda7d9c0 | 43 | pc.baud(115200); |
jdawkins | 0:42d1dda7d9c0 | 44 | xbee.baud(115200); |
jdawkins | 0:42d1dda7d9c0 | 45 | |
jdawkins | 0:42d1dda7d9c0 | 46 | //imu.init_MPU_i2c(); |
jdawkins | 0:42d1dda7d9c0 | 47 | left = 0; |
jdawkins | 0:42d1dda7d9c0 | 48 | str_cmd = 0; |
jdawkins | 0:42d1dda7d9c0 | 49 | str=0; |
jdawkins | 0:42d1dda7d9c0 | 50 | thr=0; |
jdawkins | 0:42d1dda7d9c0 | 51 | thr_cmd = 0; |
jdawkins | 0:42d1dda7d9c0 | 52 | arm_clock =0; |
jdawkins | 0:42d1dda7d9c0 | 53 | str_cond = false; |
jdawkins | 0:42d1dda7d9c0 | 54 | thr_cond = false; |
jdawkins | 0:42d1dda7d9c0 | 55 | armed = false; |
jdawkins | 0:42d1dda7d9c0 | 56 | auto_ctrl = false; |
jdawkins | 0:42d1dda7d9c0 | 57 | run_ctrl = false; |
jdawkins | 0:42d1dda7d9c0 | 58 | log_data = false; |
jdawkins | 0:42d1dda7d9c0 | 59 | |
jdawkins | 0:42d1dda7d9c0 | 60 | t.start(); |
jdawkins | 0:42d1dda7d9c0 | 61 | t_imu = t.read(); |
jdawkins | 0:42d1dda7d9c0 | 62 | t_cmd = 0; |
jdawkins | 0:42d1dda7d9c0 | 63 | |
jdawkins | 0:42d1dda7d9c0 | 64 | status_LED = 1; |
jdawkins | 0:42d1dda7d9c0 | 65 | |
jdawkins | 0:42d1dda7d9c0 | 66 | if(imu.check()) { |
jdawkins | 3:82e223a4a4e4 | 67 | |
jdawkins | 0:42d1dda7d9c0 | 68 | pc.printf("BNO055 connected\r\n"); |
jdawkins | 0:42d1dda7d9c0 | 69 | imu.setmode(OPERATION_MODE_CONFIG); |
jdawkins | 0:42d1dda7d9c0 | 70 | imu.SetExternalCrystal(1); |
jdawkins | 0:42d1dda7d9c0 | 71 | imu.setmode(OPERATION_MODE_NDOF); //Uses magnetometer |
jdawkins | 3:82e223a4a4e4 | 72 | imu.set_angle_units(DEGREES); |
jdawkins | 3:82e223a4a4e4 | 73 | imu.set_accel_units(MPERSPERS); |
jdawkins | 3:82e223a4a4e4 | 74 | imu.set_anglerate_units(DEG_PER_SEC); |
jdawkins | 3:82e223a4a4e4 | 75 | imu.setoutputformat(WINDOWS); |
jdawkins | 2:899128d20215 | 76 | imu.set_mapping(2); |
jdawkins | 3:82e223a4a4e4 | 77 | |
jdawkins | 2:899128d20215 | 78 | |
jdawkins | 2:899128d20215 | 79 | |
jdawkins | 0:42d1dda7d9c0 | 80 | } else { |
jdawkins | 0:42d1dda7d9c0 | 81 | pc.printf("IMU BNO055 NOT connected\r\n Program Trap."); |
jdawkins | 0:42d1dda7d9c0 | 82 | status_LED = 1; |
jdawkins | 0:42d1dda7d9c0 | 83 | armed_LED = 1; |
jdawkins | 2:899128d20215 | 84 | imu_LED = 1; |
jdawkins | 0:42d1dda7d9c0 | 85 | auto_LED = 1; |
jdawkins | 0:42d1dda7d9c0 | 86 | while(1) { |
jdawkins | 0:42d1dda7d9c0 | 87 | status_LED = !status_LED; |
jdawkins | 0:42d1dda7d9c0 | 88 | armed_LED = !armed_LED; |
jdawkins | 2:899128d20215 | 89 | imu_LED = !imu_LED; |
jdawkins | 0:42d1dda7d9c0 | 90 | auto_LED = !auto_LED; |
jdawkins | 0:42d1dda7d9c0 | 91 | wait(0.5); |
jdawkins | 0:42d1dda7d9c0 | 92 | } |
jdawkins | 0:42d1dda7d9c0 | 93 | } |
jdawkins | 0:42d1dda7d9c0 | 94 | |
jdawkins | 2:899128d20215 | 95 | pc.printf("ES456 Vehicle Sensor Logger\r\n"); |
jdawkins | 0:42d1dda7d9c0 | 96 | while(1){ |
jdawkins | 3:82e223a4a4e4 | 97 | |
jdawkins | 0:42d1dda7d9c0 | 98 | imu.get_angles(); |
jdawkins | 0:42d1dda7d9c0 | 99 | imu.get_accel(); |
jdawkins | 0:42d1dda7d9c0 | 100 | imu.get_gyro(); |
jdawkins | 0:42d1dda7d9c0 | 101 | imu.get_mag(); |
jdawkins | 0:42d1dda7d9c0 | 102 | if(t.read()-t_imu > (1/LOG_RATE)) { |
jdawkins | 0:42d1dda7d9c0 | 103 | |
jdawkins | 3:82e223a4a4e4 | 104 | |
jdawkins | 3:82e223a4a4e4 | 105 | //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); |
jdawkins | 3:82e223a4a4e4 | 106 | //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); |
jdawkins | 3:82e223a4a4e4 | 107 | |
jdawkins | 0:42d1dda7d9c0 | 108 | 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); |
jdawkins | 0:42d1dda7d9c0 | 109 | pc.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z); |
jdawkins | 0:42d1dda7d9c0 | 110 | |
jdawkins | 3:82e223a4a4e4 | 111 | // 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); |
jdawkins | 3:82e223a4a4e4 | 112 | // xbee.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z); |
jdawkins | 0:42d1dda7d9c0 | 113 | |
jdawkins | 0:42d1dda7d9c0 | 114 | t_imu = t.read(); |
jdawkins | 0:42d1dda7d9c0 | 115 | } // if t.read |
jdawkins | 0:42d1dda7d9c0 | 116 | wait(1/LOOP_RATE); |
jdawkins | 0:42d1dda7d9c0 | 117 | status_LED=!status_LED; |
jdawkins | 0:42d1dda7d9c0 | 118 | } // while (1) |
jdawkins | 0:42d1dda7d9c0 | 119 | |
jdawkins | 0:42d1dda7d9c0 | 120 | } // main |
jdawkins | 0:42d1dda7d9c0 | 121 | |
jdawkins | 0:42d1dda7d9c0 | 122 | |
jdawkins | 0:42d1dda7d9c0 | 123 |