Ben, Simon, Inez IDD / Mbed 2 deprecated Doom_Controller

Renamed to program to Doom_Controller.

Dependencies:   DebounceIn USBDevice mbed

Fork of BNO055_reader by Ben, Simon, Inez IDD

main.cpp@0:a59b1f819776, 2015-09-18 (annotated)

Committer:
inezraharjo
Date:
Fri Sep 18 00:40:26 2015 +0000
Revision:
0:a59b1f819776
Child:
1:38cd433ff221
v1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
inezraharjo 0:a59b1f819776 1 #include "mbed.h"
inezraharjo 0:a59b1f819776 2 #include "USBMouseKeyboard.h"
inezraharjo 0:a59b1f819776 3 //LEDs to indicate clibration status
inezraharjo 0:a59b1f819776 4 DigitalOut redLED(LED_RED);
inezraharjo 0:a59b1f819776 5 DigitalOut greenLED(LED_GREEN);
inezraharjo 0:a59b1f819776 6
inezraharjo 0:a59b1f819776 7 //pushbuttons inputs
inezraharjo 0:a59b1f819776 8 DigitalIn trigger(D11);
inezraharjo 0:a59b1f819776 9 DigitalIn move(D8);
inezraharjo 0:a59b1f819776 10 DigitalIn door(D4);
inezraharjo 0:a59b1f819776 11
inezraharjo 0:a59b1f819776 12 //USBMouseKeyboard
inezraharjo 0:a59b1f819776 13 //do we need absolute or relative mouse?
inezraharjo 0:a59b1f819776 14 USBMouseKeyboard key_mouse(ABS_MOUSE);
inezraharjo 0:a59b1f819776 15
inezraharjo 0:a59b1f819776 16 Serial pc(USBTX,USBRX);
inezraharjo 0:a59b1f819776 17 I2C i2c(D7, D6);
inezraharjo 0:a59b1f819776 18
inezraharjo 0:a59b1f819776 19 const int bno055_addr = 0x28 << 1;
inezraharjo 0:a59b1f819776 20
inezraharjo 0:a59b1f819776 21 const int BNO055_ID_ADDR = 0x00;
inezraharjo 0:a59b1f819776 22 const int BNO055_EULER_H_LSB_ADDR = 0x1A;
inezraharjo 0:a59b1f819776 23 const int BNO055_GRAVITY_DATA_X_LSB_ADDR = 0x2E;
inezraharjo 0:a59b1f819776 24 const int BNO055_TEMP_ADDR = 0x34;
inezraharjo 0:a59b1f819776 25 const int BNO055_OPR_MODE_ADDR = 0x3D;
inezraharjo 0:a59b1f819776 26 const int BNO055_CALIB_STAT_ADDR = 0x35;
inezraharjo 0:a59b1f819776 27 const int BNO055_SYS_STAT_ADDR = 0x39;
inezraharjo 0:a59b1f819776 28 const int BNO055_SYS_ERR_ADDR = 0x3A;
inezraharjo 0:a59b1f819776 29 const int BNO055_AXIS_MAP_CONFIG_ADDR = 0x41;
inezraharjo 0:a59b1f819776 30 const int BNO055_SYS_TRIGGER_ADDR = 0x3F;
inezraharjo 0:a59b1f819776 31
inezraharjo 0:a59b1f819776 32 typedef struct CalibStatus_t
inezraharjo 0:a59b1f819776 33 {
inezraharjo 0:a59b1f819776 34 int mag;
inezraharjo 0:a59b1f819776 35 int acc;
inezraharjo 0:a59b1f819776 36 int gyr;
inezraharjo 0:a59b1f819776 37 int sys;
inezraharjo 0:a59b1f819776 38 } CalibStatus;
inezraharjo 0:a59b1f819776 39
inezraharjo 0:a59b1f819776 40 typedef struct Euler_t
inezraharjo 0:a59b1f819776 41 {
inezraharjo 0:a59b1f819776 42 float heading;
inezraharjo 0:a59b1f819776 43 float pitch;
inezraharjo 0:a59b1f819776 44 float roll;
inezraharjo 0:a59b1f819776 45 } Euler;
inezraharjo 0:a59b1f819776 46
inezraharjo 0:a59b1f819776 47 // The "zero" offset positions
inezraharjo 0:a59b1f819776 48 short int headingOffset;
inezraharjo 0:a59b1f819776 49 short int pitchOffset;
inezraharjo 0:a59b1f819776 50 short int rollOffset;
inezraharjo 0:a59b1f819776 51
inezraharjo 0:a59b1f819776 52
inezraharjo 0:a59b1f819776 53 /**
inezraharjo 0:a59b1f819776 54 * Function to write to a single 8-bit register
inezraharjo 0:a59b1f819776 55 */
inezraharjo 0:a59b1f819776 56 void writeReg(int regAddr, char value)
inezraharjo 0:a59b1f819776 57 {
inezraharjo 0:a59b1f819776 58 char wbuf[2];
inezraharjo 0:a59b1f819776 59 wbuf[0] = regAddr;
inezraharjo 0:a59b1f819776 60 wbuf[1] = value;
inezraharjo 0:a59b1f819776 61 i2c.write(bno055_addr, wbuf, 2, false);
inezraharjo 0:a59b1f819776 62 }
inezraharjo 0:a59b1f819776 63
inezraharjo 0:a59b1f819776 64 /**
inezraharjo 0:a59b1f819776 65 * Function to read from a single 8-bit register
inezraharjo 0:a59b1f819776 66 */
inezraharjo 0:a59b1f819776 67 char readReg(int regAddr)
inezraharjo 0:a59b1f819776 68 {
inezraharjo 0:a59b1f819776 69 char rwbuf = regAddr;
inezraharjo 0:a59b1f819776 70 i2c.write(bno055_addr, &rwbuf, 1, false);
inezraharjo 0:a59b1f819776 71 i2c.read(bno055_addr, &rwbuf, 1, false);
inezraharjo 0:a59b1f819776 72 return rwbuf;
inezraharjo 0:a59b1f819776 73 }
inezraharjo 0:a59b1f819776 74
inezraharjo 0:a59b1f819776 75 /**
inezraharjo 0:a59b1f819776 76 * Returns the calibration status of each component
inezraharjo 0:a59b1f819776 77 */
inezraharjo 0:a59b1f819776 78 CalibStatus readCalibrationStatus()
inezraharjo 0:a59b1f819776 79 {
inezraharjo 0:a59b1f819776 80 CalibStatus status;
inezraharjo 0:a59b1f819776 81 int regVal = readReg(BNO055_CALIB_STAT_ADDR);
inezraharjo 0:a59b1f819776 82
inezraharjo 0:a59b1f819776 83 status.mag = regVal & 0x03;
inezraharjo 0:a59b1f819776 84 status.acc = (regVal >> 2) & 0x03;
inezraharjo 0:a59b1f819776 85 status.gyr = (regVal >> 4) & 0x03;
inezraharjo 0:a59b1f819776 86 status.sys = (regVal >> 6) & 0x03;
inezraharjo 0:a59b1f819776 87
inezraharjo 0:a59b1f819776 88 return status;
inezraharjo 0:a59b1f819776 89 }
inezraharjo 0:a59b1f819776 90
inezraharjo 0:a59b1f819776 91
inezraharjo 0:a59b1f819776 92 /**
inezraharjo 0:a59b1f819776 93 * Returns true if all the devices are calibrated
inezraharjo 0:a59b1f819776 94 */
inezraharjo 0:a59b1f819776 95 bool calibrated()
inezraharjo 0:a59b1f819776 96 {
inezraharjo 0:a59b1f819776 97 CalibStatus status = readCalibrationStatus();
inezraharjo 0:a59b1f819776 98
inezraharjo 0:a59b1f819776 99 if(status.mag == 3 && status.acc == 3 && status.gyr == 3)
inezraharjo 0:a59b1f819776 100 return true;
inezraharjo 0:a59b1f819776 101 else
inezraharjo 0:a59b1f819776 102 return false;
inezraharjo 0:a59b1f819776 103 }
inezraharjo 0:a59b1f819776 104
inezraharjo 0:a59b1f819776 105
inezraharjo 0:a59b1f819776 106 /**
inezraharjo 0:a59b1f819776 107 * Checks that there are no errors on the accelerometer
inezraharjo 0:a59b1f819776 108 */
inezraharjo 0:a59b1f819776 109 bool bno055Healthy()
inezraharjo 0:a59b1f819776 110 {
inezraharjo 0:a59b1f819776 111 int sys_error = readReg(BNO055_SYS_ERR_ADDR);
inezraharjo 0:a59b1f819776 112 wait(0.001);
inezraharjo 0:a59b1f819776 113 int sys_stat = readReg(BNO055_SYS_STAT_ADDR);
inezraharjo 0:a59b1f819776 114 wait(0.001);
inezraharjo 0:a59b1f819776 115
inezraharjo 0:a59b1f819776 116 if(sys_error == 0 && sys_stat == 5)
inezraharjo 0:a59b1f819776 117 return true;
inezraharjo 0:a59b1f819776 118 else {
inezraharjo 0:a59b1f819776 119 //pc.printf("SYS_ERR: %d SYS_STAT: %d\r\n", sys_error, sys_stat);
inezraharjo 0:a59b1f819776 120 return false;
inezraharjo 0:a59b1f819776 121 }
inezraharjo 0:a59b1f819776 122 }
inezraharjo 0:a59b1f819776 123
inezraharjo 0:a59b1f819776 124
inezraharjo 0:a59b1f819776 125 /**
inezraharjo 0:a59b1f819776 126 * Configure and initialize the BNO055
inezraharjo 0:a59b1f819776 127 */
inezraharjo 0:a59b1f819776 128 bool initBNO055()
inezraharjo 0:a59b1f819776 129 {
inezraharjo 0:a59b1f819776 130 unsigned char regVal;
inezraharjo 0:a59b1f819776 131 i2c.frequency(400000);
inezraharjo 0:a59b1f819776 132 bool startupPass = true;
inezraharjo 0:a59b1f819776 133
inezraharjo 0:a59b1f819776 134 // Do some basic power-up tests
inezraharjo 0:a59b1f819776 135 regVal = readReg(BNO055_ID_ADDR);
inezraharjo 0:a59b1f819776 136 if(regVal == 0xA0)
inezraharjo 0:a59b1f819776 137 pc.printf("BNO055 successfully detected!\r\n");
inezraharjo 0:a59b1f819776 138 else {
inezraharjo 0:a59b1f819776 139 pc.printf("ERROR: no BNO055 detected\r\n");
inezraharjo 0:a59b1f819776 140 startupPass = false;
inezraharjo 0:a59b1f819776 141 }
inezraharjo 0:a59b1f819776 142
inezraharjo 0:a59b1f819776 143 regVal = readReg(BNO055_TEMP_ADDR);
inezraharjo 0:a59b1f819776 144 pc.printf("Chip temperature is: %d C\r\n", regVal);
inezraharjo 0:a59b1f819776 145
inezraharjo 0:a59b1f819776 146 if(regVal == 0)
inezraharjo 0:a59b1f819776 147 startupPass = false;
inezraharjo 0:a59b1f819776 148
inezraharjo 0:a59b1f819776 149 // Change mode to CONFIG
inezraharjo 0:a59b1f819776 150 writeReg(BNO055_OPR_MODE_ADDR, 0x00);
inezraharjo 0:a59b1f819776 151 wait(0.2);
inezraharjo 0:a59b1f819776 152
inezraharjo 0:a59b1f819776 153 regVal = readReg(BNO055_OPR_MODE_ADDR);
inezraharjo 0:a59b1f819776 154 pc.printf("Change to mode: %d\r\n", regVal);
inezraharjo 0:a59b1f819776 155 wait(0.1);
inezraharjo 0:a59b1f819776 156
inezraharjo 0:a59b1f819776 157 // Remap axes
inezraharjo 0:a59b1f819776 158 writeReg(BNO055_AXIS_MAP_CONFIG_ADDR, 0x06); // b00_00_01_10
inezraharjo 0:a59b1f819776 159 wait(0.1);
inezraharjo 0:a59b1f819776 160
inezraharjo 0:a59b1f819776 161 // Set to external crystal
inezraharjo 0:a59b1f819776 162 writeReg(BNO055_SYS_TRIGGER_ADDR, 0x80);
inezraharjo 0:a59b1f819776 163 wait(0.2);
inezraharjo 0:a59b1f819776 164
inezraharjo 0:a59b1f819776 165 // Change mode to NDOF
inezraharjo 0:a59b1f819776 166 writeReg(BNO055_OPR_MODE_ADDR, 0x0C);
inezraharjo 0:a59b1f819776 167 wait(0.2);
inezraharjo 0:a59b1f819776 168
inezraharjo 0:a59b1f819776 169 regVal = readReg(BNO055_OPR_MODE_ADDR);
inezraharjo 0:a59b1f819776 170 pc.printf("Change to mode: %d\r\n", regVal);
inezraharjo 0:a59b1f819776 171 wait(0.1);
inezraharjo 0:a59b1f819776 172
inezraharjo 0:a59b1f819776 173 return startupPass;
inezraharjo 0:a59b1f819776 174 }
inezraharjo 0:a59b1f819776 175
inezraharjo 0:a59b1f819776 176 /**
inezraharjo 0:a59b1f819776 177 * Sets the current accelerometer position as the zero position.
inezraharjo 0:a59b1f819776 178 */
inezraharjo 0:a59b1f819776 179 void setZeroPosition()
inezraharjo 0:a59b1f819776 180 {
inezraharjo 0:a59b1f819776 181 char buf[16];
inezraharjo 0:a59b1f819776 182
inezraharjo 0:a59b1f819776 183 // Read the current euler angles and set them as the zero position
inezraharjo 0:a59b1f819776 184 buf[0] = BNO055_EULER_H_LSB_ADDR;
inezraharjo 0:a59b1f819776 185 i2c.write(bno055_addr, buf, 1, false);
inezraharjo 0:a59b1f819776 186 i2c.read(bno055_addr, buf, 6, false);
inezraharjo 0:a59b1f819776 187
inezraharjo 0:a59b1f819776 188 headingOffset = buf[0] + (buf[1] << 8);
inezraharjo 0:a59b1f819776 189 rollOffset = buf[2] + (buf[3] << 8);
inezraharjo 0:a59b1f819776 190 pitchOffset = buf[4] + (buf[5] << 8);
inezraharjo 0:a59b1f819776 191 }
inezraharjo 0:a59b1f819776 192
inezraharjo 0:a59b1f819776 193
inezraharjo 0:a59b1f819776 194 /**
inezraharjo 0:a59b1f819776 195 * Reads the Euler angles, zeroed out
inezraharjo 0:a59b1f819776 196 */
inezraharjo 0:a59b1f819776 197 Euler getEulerAngles()
inezraharjo 0:a59b1f819776 198 {
inezraharjo 0:a59b1f819776 199 char buf[16];
inezraharjo 0:a59b1f819776 200 Euler e;
inezraharjo 0:a59b1f819776 201
inezraharjo 0:a59b1f819776 202 // Read in the Euler angles
inezraharjo 0:a59b1f819776 203 buf[0] = BNO055_EULER_H_LSB_ADDR;
inezraharjo 0:a59b1f819776 204 i2c.write(bno055_addr, buf, 1, false);
inezraharjo 0:a59b1f819776 205 i2c.read(bno055_addr, buf, 6, false);
inezraharjo 0:a59b1f819776 206
inezraharjo 0:a59b1f819776 207 short int euler_head = buf[0] + (buf[1] << 8);
inezraharjo 0:a59b1f819776 208 short int euler_roll = buf[2] + (buf[3] << 8);
inezraharjo 0:a59b1f819776 209 short int euler_pitch = buf[4] + (buf[5] << 8);
inezraharjo 0:a59b1f819776 210
inezraharjo 0:a59b1f819776 211 e.heading = ((int)euler_head - (int)headingOffset) / 16.0;
inezraharjo 0:a59b1f819776 212 e.roll = ((int)euler_roll - (int)rollOffset) / 16.0;
inezraharjo 0:a59b1f819776 213 e.pitch = ((int)euler_pitch - (int)pitchOffset) / 16.0;
inezraharjo 0:a59b1f819776 214
inezraharjo 0:a59b1f819776 215 return e;
inezraharjo 0:a59b1f819776 216 }
inezraharjo 0:a59b1f819776 217
inezraharjo 0:a59b1f819776 218
inezraharjo 0:a59b1f819776 219 int main() {
inezraharjo 0:a59b1f819776 220
inezraharjo 0:a59b1f819776 221 uint16_t x_center = (X_MAX_ABS - X_MIN_ABS)/2;
inezraharjo 0:a59b1f819776 222 uint16_t y_center = (Y_MAX_ABS - Y_MIN_ABS)/2;
inezraharjo 0:a59b1f819776 223 uint16_t x_screen = 0;
inezraharjo 0:a59b1f819776 224 uint16_t y_screen = 0;
inezraharjo 0:a59b1f819776 225
inezraharjo 0:a59b1f819776 226 //uint32_t x_origin = x_center;
inezraharjo 0:a59b1f819776 227 //uint32_t y_origin = y_center;
inezraharjo 0:a59b1f819776 228 //uint32_t radius = 5000;
inezraharjo 0:a59b1f819776 229 //uint32_t angle = 0;
inezraharjo 0:a59b1f819776 230
inezraharjo 0:a59b1f819776 231 redLED = 0;
inezraharjo 0:a59b1f819776 232 bool startupPassed;
inezraharjo 0:a59b1f819776 233 Euler e;
inezraharjo 0:a59b1f819776 234 bool down;
inezraharjo 0:a59b1f819776 235
inezraharjo 0:a59b1f819776 236 // Initialize
inezraharjo 0:a59b1f819776 237 pc.baud(115200);
inezraharjo 0:a59b1f819776 238 trigger.mode(PullUp);
inezraharjo 0:a59b1f819776 239 move.mode(PullUp);
inezraharjo 0:a59b1f819776 240 door.mode(PullUp);
inezraharjo 0:a59b1f819776 241 wait(0.8);
inezraharjo 0:a59b1f819776 242 startupPassed = initBNO055(); // Note: set LED to RED if this fails
inezraharjo 0:a59b1f819776 243
inezraharjo 0:a59b1f819776 244 // Wait until calibration passes
inezraharjo 0:a59b1f819776 245
inezraharjo 0:a59b1f819776 246 while(!calibrated()){
inezraharjo 0:a59b1f819776 247 wait(0.1);
inezraharjo 0:a59b1f819776 248 CalibStatus calStat = readCalibrationStatus();
inezraharjo 0:a59b1f819776 249 printf("MAG: %d ACC: %d GYR: %d SYS: %d\r\n", calStat.mag, calStat.acc, calStat.gyr, calStat.sys);
inezraharjo 0:a59b1f819776 250 wait(0.5);
inezraharjo 0:a59b1f819776 251 }
inezraharjo 0:a59b1f819776 252 redLED = 1;
inezraharjo 0:a59b1f819776 253 greenLED = 0;
inezraharjo 0:a59b1f819776 254
inezraharjo 0:a59b1f819776 255 pc.printf("Board fully calibrated!\r\n");
inezraharjo 0:a59b1f819776 256
inezraharjo 0:a59b1f819776 257 // Wait until user hits the trigger. Then zero out the readings
inezraharjo 0:a59b1f819776 258 while(trigger == 1) {
inezraharjo 0:a59b1f819776 259 wait(0.01);
inezraharjo 0:a59b1f819776 260 }
inezraharjo 0:a59b1f819776 261 setZeroPosition();
inezraharjo 0:a59b1f819776 262
inezraharjo 0:a59b1f819776 263 // Read orientation values
inezraharjo 0:a59b1f819776 264 while(true)
inezraharjo 0:a59b1f819776 265 {
inezraharjo 0:a59b1f819776 266 // Make sure that there are no errors
inezraharjo 0:a59b1f819776 267 if(!bno055Healthy())
inezraharjo 0:a59b1f819776 268 wait(0.1);
inezraharjo 0:a59b1f819776 269 CalibStatus calStat = readCalibrationStatus();
inezraharjo 0:a59b1f819776 270 pc.printf("Heading: %7.2f \tRoll: %7.2f \tPitch: %7.2f Down: %d \tMAG: %d ACC: %d GYR: %d SYS: %d\r\n", e.heading, e.roll, e.pitch,down, calStat.mag, calStat.acc, calStat.gyr, calStat.sys);
inezraharjo 0:a59b1f819776 271 pc.printf("ERROR: BNO055 has an error/status problem!!!\r\n");
inezraharjo 0:a59b1f819776 272
inezraharjo 0:a59b1f819776 273 // Read in the Euler angles
inezraharjo 0:a59b1f819776 274 e = getEulerAngles();
inezraharjo 0:a59b1f819776 275 wait(0.001);
inezraharjo 0:a59b1f819776 276
inezraharjo 0:a59b1f819776 277 // Read in the calibration status
inezraharjo 0:a59b1f819776 278 calStat = readCalibrationStatus();
inezraharjo 0:a59b1f819776 279 wait(0.001);
inezraharjo 0:a59b1f819776 280
inezraharjo 0:a59b1f819776 281 // Check if device is pointing down
inezraharjo 0:a59b1f819776 282 down = (e.pitch < -70);
inezraharjo 0:a59b1f819776 283
inezraharjo 0:a59b1f819776 284 //if it is down, then change device
inezraharjo 0:a59b1f819776 285 if (down){
inezraharjo 0:a59b1f819776 286 key_mouse.click(MOUSE_RIGHT);
inezraharjo 0:a59b1f819776 287 wait(0.3); //do we need to change the wait time
inezraharjo 0:a59b1f819776 288 }
inezraharjo 0:a59b1f819776 289 if (!trigger){
inezraharjo 0:a59b1f819776 290 key_mouse.click(MOUSE_LEFT);
inezraharjo 0:a59b1f819776 291 wait(0.05); //0.3 was too long
inezraharjo 0:a59b1f819776 292 }
inezraharjo 0:a59b1f819776 293 if (!door){
inezraharjo 0:a59b1f819776 294 int counter = 0;
inezraharjo 0:a59b1f819776 295 while(counter<50){
inezraharjo 0:a59b1f819776 296 key_mouse.keyCode(' ');
inezraharjo 0:a59b1f819776 297 counter++;
inezraharjo 0:a59b1f819776 298 }
inezraharjo 0:a59b1f819776 299 wait(0.5);
inezraharjo 0:a59b1f819776 300 }
inezraharjo 0:a59b1f819776 301 if (!move){
inezraharjo 0:a59b1f819776 302 int count = 0;
inezraharjo 0:a59b1f819776 303 while (count<100){
inezraharjo 0:a59b1f819776 304 key_mouse.keyCode('a');
inezraharjo 0:a59b1f819776 305 count++;
inezraharjo 0:a59b1f819776 306 }
inezraharjo 0:a59b1f819776 307 }
inezraharjo 0:a59b1f819776 308 //moving the mouse now
inezraharjo 0:a59b1f819776 309
inezraharjo 0:a59b1f819776 310 x_screen = x_center+(e.heading/180*(X_MAX_ABS-x_center)); //45 was too sensitive
inezraharjo 0:a59b1f819776 311 y_screen = y_center;
inezraharjo 0:a59b1f819776 312
inezraharjo 0:a59b1f819776 313 //printf("Heading: %7.2f \tRoll: %7.2f \tPitch: %7.2f Down: %d \tMAG: %d ACC: %d GYR: %d SYS: %d\r\n", e.heading, e.roll, e.pitch,
inezraharjo 0:a59b1f819776 314 // down, calStat.mag, calStat.acc, calStat.gyr, calStat.sys);
inezraharjo 0:a59b1f819776 315
inezraharjo 0:a59b1f819776 316 key_mouse.move(x_screen, y_screen);
inezraharjo 0:a59b1f819776 317 wait(0.05);
inezraharjo 0:a59b1f819776 318 }
inezraharjo 0:a59b1f819776 319 }