USNA WSE ES456 / Mbed 2 deprecated MadPulseIMU

Streams BNO055 IMU data

Dependencies:   BNO055_fusion mbed NeoStrip

Dependents:   MadPulse_Controller_ros

Fork of ES456_Labs by USNA WSE ES456

main.cpp@6:4265663c9a34, 2017-09-19 (annotated)

Committer:
jdawkins
Date:
Tue Sep 19 17:27:59 2017 +0000
Revision:
6:4265663c9a34
Parent:
5:b1c4a117eb4b
Child:
8:7f35f3cd8235
Staggered log statements for IMU messaages

Who changed what in which revision?

UserRevisionLine numberNew 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 5:b1c4a117eb4b 3 #define STAT_RATE 1.0
jdawkins 6:4265663c9a34 4 #define IMU_RATE 150.0
jdawkins 0:42d1dda7d9c0 5 #define LOOP_RATE 200.0
jdawkins 0:42d1dda7d9c0 6 #define CMD_TIMEOUT 1.0
jdawkins 0:42d1dda7d9c0 7 #define GEAR_RATIO (1/2.75)
jdawkins 0:42d1dda7d9c0 8 #define PI 3.14159
jdawkins 5:b1c4a117eb4b 9 #define LED_CLUSTERS 3
jdawkins 5:b1c4a117eb4b 10 #define LED_PER_CLUSTER 1
jdawkins 6:4265663c9a34 11 #define LOG_IMU 1
jdawkins 6:4265663c9a34 12 #define LOG_MAG 2
jdawkins 6:4265663c9a34 13 #define LOG_EUL 3
jdawkins 6:4265663c9a34 14 #define LOG_ODO 4
jdawkins 6:4265663c9a34 15
jdawkins 0:42d1dda7d9c0 16 #include "mbed.h"
jdawkins 0:42d1dda7d9c0 17
jdawkins 0:42d1dda7d9c0 18 #include "BNO055.h"
jdawkins 5:b1c4a117eb4b 19 #include "NeoStrip.h"
jdawkins 0:42d1dda7d9c0 20
jdawkins 0:42d1dda7d9c0 21
jdawkins 0:42d1dda7d9c0 22 BNO055 imu(p9, p10);
jdawkins 5:b1c4a117eb4b 23 NeoStrip leds(p30,LED_CLUSTERS*LED_PER_CLUSTER);
jdawkins 0:42d1dda7d9c0 24
jdawkins 0:42d1dda7d9c0 25 int left;
jdawkins 0:42d1dda7d9c0 26 float saturateCmd(float cmd);
jdawkins 0:42d1dda7d9c0 27 void menuFunction(Serial *port);
jdawkins 0:42d1dda7d9c0 28 DigitalOut status_LED(LED1);
jdawkins 0:42d1dda7d9c0 29 DigitalOut armed_LED(LED2);
jdawkins 0:42d1dda7d9c0 30 DigitalOut auto_LED(LED3);
jdawkins 2:899128d20215 31 DigitalOut imu_LED(LED4);
jdawkins 0:42d1dda7d9c0 32
jdawkins 0:42d1dda7d9c0 33 Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
jdawkins 0:42d1dda7d9c0 34 Serial xbee(p28, p27); // tx, rx for Xbee
jdawkins 0:42d1dda7d9c0 35
jdawkins 6:4265663c9a34 36 int log_it;
jdawkins 0:42d1dda7d9c0 37
jdawkins 0:42d1dda7d9c0 38 Timer t; // create timer instance
jdawkins 5:b1c4a117eb4b 39 float t_imu,t_stat;
jdawkins 0:42d1dda7d9c0 40
jdawkins 5:b1c4a117eb4b 41 void setLED(int *colors,float brightness);
jdawkins 5:b1c4a117eb4b 42 float wrapTo2pi(float ang);
jdawkins 5:b1c4a117eb4b 43 int stat_colors[4] = {RED,ORANGE,YELLOW,GREEN};
jdawkins 5:b1c4a117eb4b 44 int colors[4];
jdawkins 0:42d1dda7d9c0 45
jdawkins 0:42d1dda7d9c0 46 int main()
jdawkins 0:42d1dda7d9c0 47 {
jdawkins 0:42d1dda7d9c0 48
jdawkins 0:42d1dda7d9c0 49 pc.baud(115200);
jdawkins 0:42d1dda7d9c0 50 xbee.baud(115200);
jdawkins 0:42d1dda7d9c0 51
jdawkins 0:42d1dda7d9c0 52
jdawkins 5:b1c4a117eb4b 53 // Initialize timers for IMU and Status Update
jdawkins 0:42d1dda7d9c0 54 t.start();
jdawkins 0:42d1dda7d9c0 55 t_imu = t.read();
jdawkins 5:b1c4a117eb4b 56 t_stat =t.read();
jdawkins 6:4265663c9a34 57 log_it = 0;
jdawkins 0:42d1dda7d9c0 58 status_LED = 1;
jdawkins 0:42d1dda7d9c0 59
jdawkins 0:42d1dda7d9c0 60 if(imu.check()) {
jdawkins 5:b1c4a117eb4b 61
jdawkins 0:42d1dda7d9c0 62 pc.printf("BNO055 connected\r\n");
jdawkins 0:42d1dda7d9c0 63 imu.setmode(OPERATION_MODE_CONFIG);
jdawkins 5:b1c4a117eb4b 64 imu.SetExternalCrystal(1);
jdawkins 5:b1c4a117eb4b 65 imu.set_angle_units(RADIANS);
jdawkins 3:82e223a4a4e4 66 imu.set_accel_units(MPERSPERS);
jdawkins 5:b1c4a117eb4b 67 imu.set_anglerate_units(RAD_PER_SEC);
jdawkins 3:82e223a4a4e4 68 imu.setoutputformat(WINDOWS);
jdawkins 2:899128d20215 69 imu.set_mapping(2);
jdawkins 5:b1c4a117eb4b 70 imu.setmode(OPERATION_MODE_NDOF); //Uses magnetometer
jdawkins 2:899128d20215 71
jdawkins 2:899128d20215 72
jdawkins 0:42d1dda7d9c0 73 } else {
jdawkins 0:42d1dda7d9c0 74 pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
jdawkins 0:42d1dda7d9c0 75 status_LED = 1;
jdawkins 0:42d1dda7d9c0 76 armed_LED = 1;
jdawkins 2:899128d20215 77 imu_LED = 1;
jdawkins 0:42d1dda7d9c0 78 auto_LED = 1;
jdawkins 0:42d1dda7d9c0 79 while(1) {
jdawkins 0:42d1dda7d9c0 80 status_LED = !status_LED;
jdawkins 0:42d1dda7d9c0 81 armed_LED = !armed_LED;
jdawkins 2:899128d20215 82 imu_LED = !imu_LED;
jdawkins 0:42d1dda7d9c0 83 auto_LED = !auto_LED;
jdawkins 0:42d1dda7d9c0 84 wait(0.5);
jdawkins 0:42d1dda7d9c0 85 }
jdawkins 0:42d1dda7d9c0 86 }
jdawkins 0:42d1dda7d9c0 87
jdawkins 2:899128d20215 88 pc.printf("ES456 Vehicle Sensor Logger\r\n");
jdawkins 0:42d1dda7d9c0 89 while(1){
jdawkins 3:82e223a4a4e4 90
jdawkins 5:b1c4a117eb4b 91
jdawkins 5:b1c4a117eb4b 92 if(t.read()-t_stat > (1/STAT_RATE)){
jdawkins 5:b1c4a117eb4b 93 uint8_t acc_stat;
jdawkins 5:b1c4a117eb4b 94 uint8_t gyro_stat;
jdawkins 5:b1c4a117eb4b 95 uint8_t mag_stat;
jdawkins 5:b1c4a117eb4b 96 uint8_t imu_stat;
jdawkins 5:b1c4a117eb4b 97 imu.get_calib();
jdawkins 5:b1c4a117eb4b 98
jdawkins 5:b1c4a117eb4b 99 // int x = (number >> (8*n)) & 0xff
jdawkins 5:b1c4a117eb4b 100
jdawkins 5:b1c4a117eb4b 101 mag_stat = (imu.calib & 0x03);
jdawkins 5:b1c4a117eb4b 102 acc_stat = (imu.calib & 0x0C)>> 2;
jdawkins 5:b1c4a117eb4b 103 gyro_stat = (imu.calib & 0x30) >> 4;
jdawkins 5:b1c4a117eb4b 104 imu_stat = (imu.calib & 0xC0) >> 6;
jdawkins 5:b1c4a117eb4b 105
jdawkins 5:b1c4a117eb4b 106 // pc.printf("%mag %d, acc %d, gyro %d imu %d\r\n",mag_stat, acc_stat, gyro_stat, imu_stat);
jdawkins 5:b1c4a117eb4b 107 colors[0] = stat_colors[mag_stat];
jdawkins 5:b1c4a117eb4b 108 colors[1] = stat_colors[acc_stat];
jdawkins 5:b1c4a117eb4b 109 colors[2] = stat_colors[gyro_stat];
jdawkins 5:b1c4a117eb4b 110 setLED(colors,0.05);
jdawkins 5:b1c4a117eb4b 111 t_stat = t.read();
jdawkins 5:b1c4a117eb4b 112 }
jdawkins 5:b1c4a117eb4b 113
jdawkins 5:b1c4a117eb4b 114 if(t.read()-t_imu > (1/IMU_RATE)) {
jdawkins 0:42d1dda7d9c0 115
jdawkins 6:4265663c9a34 116
jdawkins 6:4265663c9a34 117 switch (log_it){
jdawkins 6:4265663c9a34 118 case (LOG_IMU): {
jdawkins 6:4265663c9a34 119 imu.get_accel();
jdawkins 6:4265663c9a34 120 imu.get_gyro();
jdawkins 6:4265663c9a34 121 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 6:4265663c9a34 122
jdawkins 6:4265663c9a34 123 break;
jdawkins 6:4265663c9a34 124 }
jdawkins 6:4265663c9a34 125 case (LOG_MAG): {
jdawkins 6:4265663c9a34 126 imu.get_mag();
jdawkins 6:4265663c9a34 127 pc.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);
jdawkins 6:4265663c9a34 128 break;
jdawkins 6:4265663c9a34 129 }
jdawkins 6:4265663c9a34 130 case (LOG_EUL): {
jdawkins 6:4265663c9a34 131 imu.get_angles();
jdawkins 6:4265663c9a34 132 pc.printf("$RPY,%.3f, %.3f, %.3f\r\n", imu.euler.roll,imu.euler.pitch,wrapTo2pi(imu.euler.yaw));
jdawkins 6:4265663c9a34 133 break;
jdawkins 6:4265663c9a34 134 }
jdawkins 6:4265663c9a34 135 default :{
jdawkins 6:4265663c9a34 136 log_it = 0;
jdawkins 6:4265663c9a34 137 }
jdawkins 6:4265663c9a34 138
jdawkins 6:4265663c9a34 139 }
jdawkins 6:4265663c9a34 140 log_it++;
jdawkins 6:4265663c9a34 141
jdawkins 3:82e223a4a4e4 142
jdawkins 3:82e223a4a4e4 143 //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 144 //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 145
jdawkins 6:4265663c9a34 146
jdawkins 6:4265663c9a34 147
jdawkins 0:42d1dda7d9c0 148
jdawkins 3:82e223a4a4e4 149 // 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 150 // xbee.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);
jdawkins 0:42d1dda7d9c0 151
jdawkins 0:42d1dda7d9c0 152 t_imu = t.read();
jdawkins 0:42d1dda7d9c0 153 } // if t.read
jdawkins 0:42d1dda7d9c0 154 wait(1/LOOP_RATE);
jdawkins 0:42d1dda7d9c0 155 status_LED=!status_LED;
jdawkins 0:42d1dda7d9c0 156 } // while (1)
jdawkins 0:42d1dda7d9c0 157
jdawkins 0:42d1dda7d9c0 158 } // main
jdawkins 0:42d1dda7d9c0 159
jdawkins 5:b1c4a117eb4b 160 float wrapTo2pi(float ang){
jdawkins 5:b1c4a117eb4b 161
jdawkins 5:b1c4a117eb4b 162 if(ang > 2*PI){
jdawkins 5:b1c4a117eb4b 163 ang = ang - 2*PI;
jdawkins 5:b1c4a117eb4b 164 }
jdawkins 5:b1c4a117eb4b 165
jdawkins 5:b1c4a117eb4b 166 if(ang < 0){
jdawkins 5:b1c4a117eb4b 167 ang = ang + 2*PI;
jdawkins 5:b1c4a117eb4b 168 }
jdawkins 5:b1c4a117eb4b 169
jdawkins 5:b1c4a117eb4b 170 return ang;
jdawkins 0:42d1dda7d9c0 171
jdawkins 0:42d1dda7d9c0 172
jdawkins 5:b1c4a117eb4b 173 }
jdawkins 5:b1c4a117eb4b 174 void setLED(int *colors,float brightness)
jdawkins 5:b1c4a117eb4b 175 {
jdawkins 5:b1c4a117eb4b 176
jdawkins 5:b1c4a117eb4b 177 leds.setBrightness(brightness);
jdawkins 5:b1c4a117eb4b 178
jdawkins 5:b1c4a117eb4b 179 int cidx = 0;
jdawkins 5:b1c4a117eb4b 180 int ctr = 0;
jdawkins 5:b1c4a117eb4b 181 for (int i=0; i<LED_PER_CLUSTER*LED_CLUSTERS; i++) {
jdawkins 5:b1c4a117eb4b 182
jdawkins 5:b1c4a117eb4b 183 if(ctr >= LED_PER_CLUSTER) {
jdawkins 5:b1c4a117eb4b 184 ctr = 0;
jdawkins 5:b1c4a117eb4b 185 cidx++;
jdawkins 5:b1c4a117eb4b 186 }
jdawkins 5:b1c4a117eb4b 187 leds.setPixel(i,colors[cidx]);
jdawkins 5:b1c4a117eb4b 188 ctr++;
jdawkins 5:b1c4a117eb4b 189 }
jdawkins 5:b1c4a117eb4b 190 leds.write();
jdawkins 5:b1c4a117eb4b 191 }
jdawkins 5:b1c4a117eb4b 192
jdawkins 5:b1c4a117eb4b 193