IDD Fall 2015 / Mbed 2 deprecated idd_hw3_franklu_adithyamurali_craighiller_left

code for the left controller (Niimote)

Dependencies:   PinDetect_hw3 USBDevice mbed

acc_main.cpp@2:e3a7b0fd4a59, 2015-09-21 (annotated)

Committer:
franklu
Date:
Mon Sep 21 15:22:04 2015 +0000
Revision:
2:e3a7b0fd4a59
Parent:
1:8252a6052ebe 
changed main names;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
franklu 1:8252a6052ebe 1 #include "mbed.h"
franklu 1:8252a6052ebe 2 #include "USBKeyboard.h"
franklu 1:8252a6052ebe 3 #include "PinDetect.h"
franklu 1:8252a6052ebe 4
franklu 1:8252a6052ebe 5 /* Testing example code from Simon Scott on using BNO055. */
franklu 1:8252a6052ebe 6
franklu 1:8252a6052ebe 7 USBKeyboard keyboard;
franklu 1:8252a6052ebe 8
franklu 1:8252a6052ebe 9 Serial pc(USBTX,USBRX);
franklu 1:8252a6052ebe 10 I2C i2c(A4, A5);
franklu 1:8252a6052ebe 11
franklu 1:8252a6052ebe 12 DigitalOut led(LED1);
franklu 1:8252a6052ebe 13 DigitalOut led2(LED2);
franklu 1:8252a6052ebe 14
franklu 1:8252a6052ebe 15 const int bno055_addr = 0x28 << 1;
franklu 1:8252a6052ebe 16
franklu 1:8252a6052ebe 17 const int BNO055_ID_ADDR = 0x00;
franklu 1:8252a6052ebe 18 const int BNO055_EULER_H_LSB_ADDR = 0x1A;
franklu 1:8252a6052ebe 19 const int BNO055_TEMP_ADDR = 0x34;
franklu 1:8252a6052ebe 20 const int BNO055_OPR_MODE_ADDR = 0x3D;
franklu 1:8252a6052ebe 21 const int BNO055_CALIB_STAT_ADDR = 0x35;
franklu 1:8252a6052ebe 22 const int BNO055_SYS_STAT_ADDR = 0x39;
franklu 1:8252a6052ebe 23 const int BNO055_SYS_ERR_ADDR = 0x3A;
franklu 1:8252a6052ebe 24 const int BNO055_AXIS_MAP_CONFIG_ADDR = 0x41;
franklu 1:8252a6052ebe 25 const int BNO055_SYS_TRIGGER_ADDR = 0x3F;
franklu 1:8252a6052ebe 26
franklu 1:8252a6052ebe 27 typedef struct CalibStatus_t
franklu 1:8252a6052ebe 28 {
franklu 1:8252a6052ebe 29 int mag;
franklu 1:8252a6052ebe 30 int acc;
franklu 1:8252a6052ebe 31 int gyr;
franklu 1:8252a6052ebe 32 int sys;
franklu 1:8252a6052ebe 33 } CalibStatus;
franklu 1:8252a6052ebe 34
franklu 1:8252a6052ebe 35 typedef struct Euler_t
franklu 1:8252a6052ebe 36 {
franklu 1:8252a6052ebe 37 float heading;
franklu 1:8252a6052ebe 38 float pitch;
franklu 1:8252a6052ebe 39 float roll;
franklu 1:8252a6052ebe 40 } Euler;
franklu 1:8252a6052ebe 41
franklu 1:8252a6052ebe 42 Euler calibrationVals;
franklu 1:8252a6052ebe 43 /**
franklu 1:8252a6052ebe 44 * Function to write to a single 8-bit register
franklu 1:8252a6052ebe 45 */
franklu 1:8252a6052ebe 46 void writeReg(int regAddr, char value)
franklu 1:8252a6052ebe 47 {
franklu 1:8252a6052ebe 48 char wbuf[2];
franklu 1:8252a6052ebe 49 wbuf[0] = regAddr;
franklu 1:8252a6052ebe 50 wbuf[1] = value;
franklu 1:8252a6052ebe 51 i2c.write(bno055_addr, wbuf, 2, false);
franklu 1:8252a6052ebe 52 }
franklu 1:8252a6052ebe 53
franklu 1:8252a6052ebe 54 /**
franklu 1:8252a6052ebe 55 * Function to read from a single 8-bit register
franklu 1:8252a6052ebe 56 */
franklu 1:8252a6052ebe 57 char readReg(int regAddr)
franklu 1:8252a6052ebe 58 {
franklu 1:8252a6052ebe 59 char rwbuf = regAddr;
franklu 1:8252a6052ebe 60 i2c.write(bno055_addr, &rwbuf, 1, false);
franklu 1:8252a6052ebe 61 i2c.read(bno055_addr, &rwbuf, 1, false);
franklu 1:8252a6052ebe 62 return rwbuf;
franklu 1:8252a6052ebe 63 }
franklu 1:8252a6052ebe 64
franklu 1:8252a6052ebe 65 /**
franklu 1:8252a6052ebe 66 * Returns the calibration status of each component
franklu 1:8252a6052ebe 67 */
franklu 1:8252a6052ebe 68 CalibStatus readCalibrationStatus()
franklu 1:8252a6052ebe 69 {
franklu 1:8252a6052ebe 70 CalibStatus status;
franklu 1:8252a6052ebe 71 int regVal = readReg(BNO055_CALIB_STAT_ADDR);
franklu 1:8252a6052ebe 72
franklu 1:8252a6052ebe 73 status.mag = regVal & 0x03;
franklu 1:8252a6052ebe 74 status.acc = (regVal >> 2) & 0x03;
franklu 1:8252a6052ebe 75 status.gyr = (regVal >> 4) & 0x03;
franklu 1:8252a6052ebe 76 status.sys = (regVal >> 6) & 0x03;
franklu 1:8252a6052ebe 77
franklu 1:8252a6052ebe 78 return status;
franklu 1:8252a6052ebe 79 }
franklu 1:8252a6052ebe 80
franklu 1:8252a6052ebe 81
franklu 1:8252a6052ebe 82 /**
franklu 1:8252a6052ebe 83 * Checks that there are no errors on the accelerometer
franklu 1:8252a6052ebe 84 */
franklu 1:8252a6052ebe 85 bool bno055Healthy()
franklu 1:8252a6052ebe 86 {
franklu 1:8252a6052ebe 87 int sys_error = readReg(BNO055_SYS_ERR_ADDR);
franklu 1:8252a6052ebe 88 wait(0.001);
franklu 1:8252a6052ebe 89 int sys_stat = readReg(BNO055_SYS_STAT_ADDR);
franklu 1:8252a6052ebe 90 wait(0.001);
franklu 1:8252a6052ebe 91
franklu 1:8252a6052ebe 92 if(sys_error == 0 && sys_stat == 5)
franklu 1:8252a6052ebe 93 return true;
franklu 1:8252a6052ebe 94 else
franklu 1:8252a6052ebe 95 return false;
franklu 1:8252a6052ebe 96 }
franklu 1:8252a6052ebe 97
franklu 1:8252a6052ebe 98 /**
franklu 1:8252a6052ebe 99 * Reads the Euler angles, zeroed out
franklu 1:8252a6052ebe 100 */
franklu 1:8252a6052ebe 101 Euler getEulerAngles()
franklu 1:8252a6052ebe 102 {
franklu 1:8252a6052ebe 103 char buf[16];
franklu 1:8252a6052ebe 104 Euler e;
franklu 1:8252a6052ebe 105
franklu 1:8252a6052ebe 106 // Read in the Euler angles
franklu 1:8252a6052ebe 107 buf[0] = BNO055_EULER_H_LSB_ADDR;
franklu 1:8252a6052ebe 108 i2c.write(bno055_addr, buf, 1, false);
franklu 1:8252a6052ebe 109 i2c.read(bno055_addr, buf, 6, false);
franklu 1:8252a6052ebe 110
franklu 1:8252a6052ebe 111 short int euler_head = buf[0] + (buf[1] << 8);
franklu 1:8252a6052ebe 112 short int euler_roll = buf[2] + (buf[3] << 8);
franklu 1:8252a6052ebe 113 short int euler_pitch = buf[4] + (buf[5] << 8);
franklu 1:8252a6052ebe 114
franklu 1:8252a6052ebe 115 e.heading = ((float)euler_head) / 16.0;
franklu 1:8252a6052ebe 116 e.roll = ((float)euler_roll) / 16.0;
franklu 1:8252a6052ebe 117 e.pitch = ((float)euler_pitch) / 16.0;
franklu 1:8252a6052ebe 118
franklu 1:8252a6052ebe 119 return e;
franklu 1:8252a6052ebe 120 }
franklu 1:8252a6052ebe 121
franklu 1:8252a6052ebe 122
franklu 1:8252a6052ebe 123
franklu 1:8252a6052ebe 124 /**
franklu 1:8252a6052ebe 125 * Configure and initialize the BNO055
franklu 1:8252a6052ebe 126 */
franklu 1:8252a6052ebe 127 bool initBNO055()
franklu 1:8252a6052ebe 128 {
franklu 1:8252a6052ebe 129 unsigned char regVal;
franklu 1:8252a6052ebe 130 i2c.frequency(400000);
franklu 1:8252a6052ebe 131 bool startupPass = true;
franklu 1:8252a6052ebe 132
franklu 1:8252a6052ebe 133 // Do some basic power-up tests
franklu 1:8252a6052ebe 134 regVal = readReg(BNO055_ID_ADDR);
franklu 1:8252a6052ebe 135 if(regVal == 0xA0)
franklu 1:8252a6052ebe 136 pc.printf("BNO055 successfully detected!\r\n");
franklu 1:8252a6052ebe 137 else {
franklu 1:8252a6052ebe 138 pc.printf("ERROR: no BNO055 detected\r\n");
franklu 1:8252a6052ebe 139 startupPass = false;
franklu 1:8252a6052ebe 140 }
franklu 1:8252a6052ebe 141
franklu 1:8252a6052ebe 142 regVal = readReg(BNO055_TEMP_ADDR);
franklu 1:8252a6052ebe 143 pc.printf("Chip temperature is: %d C\r\n", regVal);
franklu 1:8252a6052ebe 144
franklu 1:8252a6052ebe 145 if(regVal == 0)
franklu 1:8252a6052ebe 146 startupPass = false;
franklu 1:8252a6052ebe 147
franklu 1:8252a6052ebe 148 // Change mode to CONFIG
franklu 1:8252a6052ebe 149 writeReg(BNO055_OPR_MODE_ADDR, 0x00);
franklu 1:8252a6052ebe 150 wait(0.2);
franklu 1:8252a6052ebe 151
franklu 1:8252a6052ebe 152 regVal = readReg(BNO055_OPR_MODE_ADDR);
franklu 1:8252a6052ebe 153 pc.printf("Change to mode: %d\r\n", regVal);
franklu 1:8252a6052ebe 154 wait(0.1);
franklu 1:8252a6052ebe 155
franklu 1:8252a6052ebe 156 // Remap axes
franklu 1:8252a6052ebe 157 writeReg(BNO055_AXIS_MAP_CONFIG_ADDR, 0x06); // b00_00_01_10
franklu 1:8252a6052ebe 158 wait(0.1);
franklu 1:8252a6052ebe 159
franklu 1:8252a6052ebe 160 // Set to external crystal
franklu 1:8252a6052ebe 161 writeReg(BNO055_SYS_TRIGGER_ADDR, 0x80);
franklu 1:8252a6052ebe 162 wait(0.2);
franklu 1:8252a6052ebe 163
franklu 1:8252a6052ebe 164 // Change mode to NDOF
franklu 1:8252a6052ebe 165 writeReg(BNO055_OPR_MODE_ADDR, 0x0C);
franklu 1:8252a6052ebe 166 wait(0.2);
franklu 1:8252a6052ebe 167
franklu 1:8252a6052ebe 168 regVal = readReg(BNO055_OPR_MODE_ADDR);
franklu 1:8252a6052ebe 169 pc.printf("Change to mode: %d\r\n", regVal);
franklu 1:8252a6052ebe 170 wait(0.1);
franklu 1:8252a6052ebe 171
franklu 1:8252a6052ebe 172 calibrationVals = getEulerAngles();
franklu 1:8252a6052ebe 173 return startupPass;
franklu 1:8252a6052ebe 174 }
franklu 1:8252a6052ebe 175
franklu 1:8252a6052ebe 176 float reduceAngle(float target, float source) {
franklu 1:8252a6052ebe 177 float a = target - source;
franklu 1:8252a6052ebe 178 if (a > 180) {
franklu 1:8252a6052ebe 179 a -= 360;
franklu 1:8252a6052ebe 180 } else if (a < -180) {
franklu 1:8252a6052ebe 181 a += 360;
franklu 1:8252a6052ebe 182 }
franklu 1:8252a6052ebe 183 return a;
franklu 1:8252a6052ebe 184 }
franklu 1:8252a6052ebe 185 int main() {
franklu 1:8252a6052ebe 186
franklu 1:8252a6052ebe 187 // button1.attach_asserted(&button1_pressed);
franklu 1:8252a6052ebe 188 // button1.attach_deasserted(&button1_released);
franklu 1:8252a6052ebe 189 // button1.setAssertValue(0);
franklu 1:8252a6052ebe 190 // button1.setSampleFrequency();
franklu 1:8252a6052ebe 191 //
franklu 1:8252a6052ebe 192 // button2.attach_asserted(&button2_pressed);
franklu 1:8252a6052ebe 193 // button2.attach_deasserted(&button2_released);
franklu 1:8252a6052ebe 194 // button2.setAssertValue(0);
franklu 1:8252a6052ebe 195 // button2.setSampleFrequency();
franklu 1:8252a6052ebe 196
franklu 1:8252a6052ebe 197 bool startupPassed;
franklu 1:8252a6052ebe 198 Euler e;
franklu 1:8252a6052ebe 199
franklu 1:8252a6052ebe 200 //RED_LED = 0;
franklu 1:8252a6052ebe 201 led = 0;
franklu 1:8252a6052ebe 202 led2 = 1;
franklu 1:8252a6052ebe 203 // Initialize
franklu 1:8252a6052ebe 204 pc.baud(115200);
franklu 1:8252a6052ebe 205 pc.printf("starting up\r\n");
franklu 1:8252a6052ebe 206 wait(0.5);
franklu 1:8252a6052ebe 207 startupPassed = initBNO055(); // Note: set LED to RED if this fails
franklu 1:8252a6052ebe 208
franklu 1:8252a6052ebe 209 float rollAvg = 0;
franklu 1:8252a6052ebe 210
franklu 1:8252a6052ebe 211 float pitchAvg = 0;
franklu 1:8252a6052ebe 212 int readCount = 0;
franklu 1:8252a6052ebe 213
franklu 1:8252a6052ebe 214 bool sentChar;
franklu 1:8252a6052ebe 215 // Read orientation values
franklu 1:8252a6052ebe 216 while(true)
franklu 1:8252a6052ebe 217 {
franklu 1:8252a6052ebe 218 sentChar = false;
franklu 1:8252a6052ebe 219 // Read in the Euler angles
franklu 1:8252a6052ebe 220 e = getEulerAngles();
franklu 1:8252a6052ebe 221
franklu 1:8252a6052ebe 222 wait(0.001);
franklu 1:8252a6052ebe 223
franklu 1:8252a6052ebe 224 // Read in the calibration status
franklu 1:8252a6052ebe 225 CalibStatus calStat = readCalibrationStatus();
franklu 1:8252a6052ebe 226
franklu 1:8252a6052ebe 227 // correct the heading, roll, pitch vals
franklu 1:8252a6052ebe 228 float corHead = reduceAngle(e.heading, calibrationVals.heading);
franklu 1:8252a6052ebe 229 float corRoll = reduceAngle(e.roll, calibrationVals.roll);
franklu 1:8252a6052ebe 230 float corPitch = reduceAngle(e.pitch, calibrationVals.pitch);
franklu 1:8252a6052ebe 231 // printf("Corrected: %7.2f \tRoll: %7.2f \tPitch: %7.2f \tMAG: %d ACC: %d GYR: %d SYS: %d\r\n", corHead, corRoll, corPitch, calStat.mag, calStat.acc, calStat.gyr, calStat.sys);
franklu 1:8252a6052ebe 232
franklu 1:8252a6052ebe 233
franklu 1:8252a6052ebe 234 rollAvg = corRoll; //.8*rollAvg + .2 * reduceAngle(corRoll, rollNums[readCount]);
franklu 1:8252a6052ebe 235 //rollNums[readCount] = corRoll;
franklu 1:8252a6052ebe 236
franklu 1:8252a6052ebe 237
franklu 1:8252a6052ebe 238 pitchAvg = corPitch;//.8*pitchAvg + .2 * reduceAngle(corPitch, pitchNums[readCount]);
franklu 1:8252a6052ebe 239 //pitchNums[readCount] = corPitch;
franklu 1:8252a6052ebe 240
franklu 1:8252a6052ebe 241 if (readCount >=5) { readCount = 0; }
franklu 1:8252a6052ebe 242 readCount++;
franklu 1:8252a6052ebe 243
franklu 1:8252a6052ebe 244 if (rollAvg < -10){
franklu 1:8252a6052ebe 245 keyboard.keyCode('z');
franklu 1:8252a6052ebe 246 keyboard.keyCode('z');
franklu 1:8252a6052ebe 247 keyboard.keyCode('z');
franklu 1:8252a6052ebe 248 keyboard.keyCode('z');
franklu 1:8252a6052ebe 249 }
franklu 1:8252a6052ebe 250
franklu 1:8252a6052ebe 251 wait(0.05);
franklu 1:8252a6052ebe 252 }
franklu 1:8252a6052ebe 253 }