USNA WSE ES456 / Mbed 2 deprecated MadPulseIMU

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BNO055_fusion mbed NeoStrip

Dependents:   MadPulse_Controller_ros

Fork of ES456_Labs by USNA WSE ES456

main.cpp@5:b1c4a117eb4b, 2017-09-08 (annotated)

Committer:
jdawkins
Date:
Fri Sep 08 19:50:34 2017 +0000
Revision:
5:b1c4a117eb4b
Parent:
3:82e223a4a4e4
Child:
6:4265663c9a34
Simplified code , changed units to rad and rad/s

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