Ion Systems / Mbed 2 deprecated triforce-control

Control code for Triforce robot.

Dependencies:   triforce-esc PwmInRC mbed

bno055.h@2:4e086a77e769, 2017-05-05 (annotated)

Committer:
IonSystems
Date:
Fri May 05 14:50:05 2017 +0000
Revision:
2:4e086a77e769
Parent:
0:d6eeeae3c3cb 
Forked ESC library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IonSystems 0:d6eeeae3c3cb 1 #include "mbed.h"
IonSystems 0:d6eeeae3c3cb 2 I2C i2c(p9, p10);
IonSystems 0:d6eeeae3c3cb 3
IonSystems 0:d6eeeae3c3cb 4 const int bno055_addr = 0x28 << 1;
IonSystems 0:d6eeeae3c3cb 5
IonSystems 0:d6eeeae3c3cb 6 const int BNO055_ID_ADDR = 0x00;
IonSystems 0:d6eeeae3c3cb 7 const int BNO055_EULER_H_LSB_ADDR = 0x1A;
IonSystems 0:d6eeeae3c3cb 8 const int BNO055_TEMP_ADDR = 0x34;
IonSystems 0:d6eeeae3c3cb 9 const int BNO055_OPR_MODE_ADDR = 0x3D;
IonSystems 0:d6eeeae3c3cb 10 const int BNO055_CALIB_STAT_ADDR = 0x35;
IonSystems 0:d6eeeae3c3cb 11 const int BNO055_SYS_STAT_ADDR = 0x39;
IonSystems 0:d6eeeae3c3cb 12 const int BNO055_SYS_ERR_ADDR = 0x3A;
IonSystems 0:d6eeeae3c3cb 13 const int BNO055_AXIS_MAP_CONFIG_ADDR = 0x41;
IonSystems 0:d6eeeae3c3cb 14 const int BNO055_SYS_TRIGGER_ADDR = 0x3F;
IonSystems 0:d6eeeae3c3cb 15
IonSystems 0:d6eeeae3c3cb 16 typedef struct CalibStatus_t
IonSystems 0:d6eeeae3c3cb 17 {
IonSystems 0:d6eeeae3c3cb 18 int mag;
IonSystems 0:d6eeeae3c3cb 19 int acc;
IonSystems 0:d6eeeae3c3cb 20 int gyr;
IonSystems 0:d6eeeae3c3cb 21 int sys;
IonSystems 0:d6eeeae3c3cb 22 } CalibStatus;
IonSystems 0:d6eeeae3c3cb 23
IonSystems 0:d6eeeae3c3cb 24 typedef struct Euler_t
IonSystems 0:d6eeeae3c3cb 25 {
IonSystems 0:d6eeeae3c3cb 26 float heading;
IonSystems 0:d6eeeae3c3cb 27 float pitch;
IonSystems 0:d6eeeae3c3cb 28 float roll;
IonSystems 0:d6eeeae3c3cb 29 } Euler;
IonSystems 0:d6eeeae3c3cb 30
IonSystems 0:d6eeeae3c3cb 31
IonSystems 0:d6eeeae3c3cb 32 /**
IonSystems 0:d6eeeae3c3cb 33 * Function to write to a single 8-bit register
IonSystems 0:d6eeeae3c3cb 34 */
IonSystems 0:d6eeeae3c3cb 35 void writeReg(int regAddr, char value)
IonSystems 0:d6eeeae3c3cb 36 {
IonSystems 0:d6eeeae3c3cb 37 char wbuf[2];
IonSystems 0:d6eeeae3c3cb 38 wbuf[0] = regAddr;
IonSystems 0:d6eeeae3c3cb 39 wbuf[1] = value;
IonSystems 0:d6eeeae3c3cb 40 i2c.write(bno055_addr, wbuf, 2, false);
IonSystems 0:d6eeeae3c3cb 41 }
IonSystems 0:d6eeeae3c3cb 42
IonSystems 0:d6eeeae3c3cb 43 /**
IonSystems 0:d6eeeae3c3cb 44 * Function to read from a single 8-bit register
IonSystems 0:d6eeeae3c3cb 45 */
IonSystems 0:d6eeeae3c3cb 46 char readReg(int regAddr)
IonSystems 0:d6eeeae3c3cb 47 {
IonSystems 0:d6eeeae3c3cb 48 char rwbuf = regAddr;
IonSystems 0:d6eeeae3c3cb 49 i2c.write(bno055_addr, &rwbuf, 1, false);
IonSystems 0:d6eeeae3c3cb 50 i2c.read(bno055_addr, &rwbuf, 1, false);
IonSystems 0:d6eeeae3c3cb 51 return rwbuf;
IonSystems 0:d6eeeae3c3cb 52 }
IonSystems 0:d6eeeae3c3cb 53
IonSystems 0:d6eeeae3c3cb 54 /**
IonSystems 0:d6eeeae3c3cb 55 * Returns the calibration status of each component
IonSystems 0:d6eeeae3c3cb 56 */
IonSystems 0:d6eeeae3c3cb 57 CalibStatus readCalibrationStatus()
IonSystems 0:d6eeeae3c3cb 58 {
IonSystems 0:d6eeeae3c3cb 59 CalibStatus status;
IonSystems 0:d6eeeae3c3cb 60 int regVal = readReg(BNO055_CALIB_STAT_ADDR);
IonSystems 0:d6eeeae3c3cb 61
IonSystems 0:d6eeeae3c3cb 62 status.mag = regVal & 0x03;
IonSystems 0:d6eeeae3c3cb 63 status.acc = (regVal >> 2) & 0x03;
IonSystems 0:d6eeeae3c3cb 64 status.gyr = (regVal >> 4) & 0x03;
IonSystems 0:d6eeeae3c3cb 65 status.sys = (regVal >> 6) & 0x03;
IonSystems 0:d6eeeae3c3cb 66
IonSystems 0:d6eeeae3c3cb 67 return status;
IonSystems 0:d6eeeae3c3cb 68 }
IonSystems 0:d6eeeae3c3cb 69
IonSystems 0:d6eeeae3c3cb 70
IonSystems 0:d6eeeae3c3cb 71 /**
IonSystems 0:d6eeeae3c3cb 72 * Checks that there are no errors on the accelerometer
IonSystems 0:d6eeeae3c3cb 73 */
IonSystems 0:d6eeeae3c3cb 74 bool bno055Healthy()
IonSystems 0:d6eeeae3c3cb 75 {
IonSystems 0:d6eeeae3c3cb 76 int sys_error = readReg(BNO055_SYS_ERR_ADDR);
IonSystems 0:d6eeeae3c3cb 77 wait(0.001);
IonSystems 0:d6eeeae3c3cb 78 int sys_stat = readReg(BNO055_SYS_STAT_ADDR);
IonSystems 0:d6eeeae3c3cb 79 wait(0.001);
IonSystems 0:d6eeeae3c3cb 80
IonSystems 0:d6eeeae3c3cb 81 if(sys_error == 0 && sys_stat == 5)
IonSystems 0:d6eeeae3c3cb 82 return true;
IonSystems 0:d6eeeae3c3cb 83 else
IonSystems 0:d6eeeae3c3cb 84 return false;
IonSystems 0:d6eeeae3c3cb 85 }
IonSystems 0:d6eeeae3c3cb 86
IonSystems 0:d6eeeae3c3cb 87
IonSystems 0:d6eeeae3c3cb 88 /**
IonSystems 0:d6eeeae3c3cb 89 * Configure and initialize the BNO055
IonSystems 0:d6eeeae3c3cb 90 */
IonSystems 0:d6eeeae3c3cb 91 bool initBNO055()
IonSystems 0:d6eeeae3c3cb 92 {
IonSystems 0:d6eeeae3c3cb 93 unsigned char regVal;
IonSystems 0:d6eeeae3c3cb 94 i2c.frequency(400000);
IonSystems 0:d6eeeae3c3cb 95 bool startupPass = true;
IonSystems 0:d6eeeae3c3cb 96
IonSystems 0:d6eeeae3c3cb 97 // Do some basic power-up tests
IonSystems 0:d6eeeae3c3cb 98 regVal = readReg(BNO055_ID_ADDR);
IonSystems 0:d6eeeae3c3cb 99 if(regVal != 0xA0){
IonSystems 0:d6eeeae3c3cb 100 startupPass = false;
IonSystems 0:d6eeeae3c3cb 101 }
IonSystems 0:d6eeeae3c3cb 102
IonSystems 0:d6eeeae3c3cb 103 regVal = readReg(BNO055_TEMP_ADDR);
IonSystems 0:d6eeeae3c3cb 104
IonSystems 0:d6eeeae3c3cb 105 if(regVal == 0)
IonSystems 0:d6eeeae3c3cb 106 startupPass = false;
IonSystems 0:d6eeeae3c3cb 107
IonSystems 0:d6eeeae3c3cb 108 // Change mode to CONFIG
IonSystems 0:d6eeeae3c3cb 109 writeReg(BNO055_OPR_MODE_ADDR, 0x00);
IonSystems 0:d6eeeae3c3cb 110 wait(0.2);
IonSystems 0:d6eeeae3c3cb 111
IonSystems 0:d6eeeae3c3cb 112 regVal = readReg(BNO055_OPR_MODE_ADDR);
IonSystems 0:d6eeeae3c3cb 113 wait(0.1);
IonSystems 0:d6eeeae3c3cb 114
IonSystems 0:d6eeeae3c3cb 115 // Remap axes
IonSystems 0:d6eeeae3c3cb 116 writeReg(BNO055_AXIS_MAP_CONFIG_ADDR, 0x06); // b00_00_01_10
IonSystems 0:d6eeeae3c3cb 117 wait(0.1);
IonSystems 0:d6eeeae3c3cb 118
IonSystems 0:d6eeeae3c3cb 119 // Set to external crystal
IonSystems 0:d6eeeae3c3cb 120 writeReg(BNO055_SYS_TRIGGER_ADDR, 0x80);
IonSystems 0:d6eeeae3c3cb 121 wait(0.2);
IonSystems 0:d6eeeae3c3cb 122
IonSystems 0:d6eeeae3c3cb 123 // Change mode to NDOF
IonSystems 0:d6eeeae3c3cb 124 writeReg(BNO055_OPR_MODE_ADDR, 0x0C);
IonSystems 0:d6eeeae3c3cb 125 wait(0.2);
IonSystems 0:d6eeeae3c3cb 126
IonSystems 0:d6eeeae3c3cb 127 regVal = readReg(BNO055_OPR_MODE_ADDR);
IonSystems 0:d6eeeae3c3cb 128 wait(0.1);
IonSystems 0:d6eeeae3c3cb 129
IonSystems 0:d6eeeae3c3cb 130 return startupPass;
IonSystems 0:d6eeeae3c3cb 131 }
IonSystems 0:d6eeeae3c3cb 132
IonSystems 0:d6eeeae3c3cb 133 /**
IonSystems 0:d6eeeae3c3cb 134 * Reads the Euler angles, zeroed out
IonSystems 0:d6eeeae3c3cb 135 */
IonSystems 0:d6eeeae3c3cb 136 Euler getEulerAngles()
IonSystems 0:d6eeeae3c3cb 137 {
IonSystems 0:d6eeeae3c3cb 138 char buf[16];
IonSystems 0:d6eeeae3c3cb 139 Euler e;
IonSystems 0:d6eeeae3c3cb 140
IonSystems 0:d6eeeae3c3cb 141 // Read in the Euler angles
IonSystems 0:d6eeeae3c3cb 142 buf[0] = BNO055_EULER_H_LSB_ADDR;
IonSystems 0:d6eeeae3c3cb 143 i2c.write(bno055_addr, buf, 1, false);
IonSystems 0:d6eeeae3c3cb 144 i2c.read(bno055_addr, buf, 6, false);
IonSystems 0:d6eeeae3c3cb 145
IonSystems 0:d6eeeae3c3cb 146 short int euler_head = buf[0] + (buf[1] << 8);
IonSystems 0:d6eeeae3c3cb 147 short int euler_roll = buf[2] + (buf[3] << 8);
IonSystems 0:d6eeeae3c3cb 148 short int euler_pitch = buf[4] + (buf[5] << 8);
IonSystems 0:d6eeeae3c3cb 149
IonSystems 0:d6eeeae3c3cb 150 e.heading = ((float)euler_head) / 16.0;
IonSystems 0:d6eeeae3c3cb 151 e.roll = ((float)euler_roll) / 16.0;
IonSystems 0:d6eeeae3c3cb 152 e.pitch = ((float)euler_pitch) / 16.0;
IonSystems 0:d6eeeae3c3cb 153
IonSystems 0:d6eeeae3c3cb 154 return e;
IonSystems 0:d6eeeae3c3cb 155 }
IonSystems 0:d6eeeae3c3cb 156
IonSystems 0:d6eeeae3c3cb 157