Ben, Simon, Inez IDD / Mbed 2 deprecated BNO055_reader

Program to simulate gun for doom using BNO055 sensor

Dependencies:   USBDevice mbed DebounceIn

main_old.txt@1:38cd433ff221, 2015-09-20 (annotated)

Committer:
simonscott
Date:
Sun Sep 20 00:58:37 2015 +0000
Revision:
1:38cd433ff221
renamed program to Doom_Controller. All works well now.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
simonscott 1:38cd433ff221 1 #include "mbed.h"
simonscott 1:38cd433ff221 2 #include "USBMouseKeyboard.h"
simonscott 1:38cd433ff221 3 #include "DebounceIn.h"
simonscott 1:38cd433ff221 4
simonscott 1:38cd433ff221 5 //LEDs to indicate clibration status
simonscott 1:38cd433ff221 6 DigitalOut redLED(LED_RED);
simonscott 1:38cd433ff221 7 DigitalOut greenLED(LED_GREEN);
simonscott 1:38cd433ff221 8
simonscott 1:38cd433ff221 9 //pushbuttons inputs
simonscott 1:38cd433ff221 10 DebounceIn trigger(D3);
simonscott 1:38cd433ff221 11 DebounceIn move(D4);
simonscott 1:38cd433ff221 12 DebounceIn door(D5);
simonscott 1:38cd433ff221 13 DebounceIn centerBut(D12);
simonscott 1:38cd433ff221 14
simonscott 1:38cd433ff221 15 // Motor output
simonscott 1:38cd433ff221 16 DigitalOut vibrate(D2);
simonscott 1:38cd433ff221 17 Timeout vibrateTimeout;
simonscott 1:38cd433ff221 18
simonscott 1:38cd433ff221 19 //USBMouseKeyboard
simonscott 1:38cd433ff221 20 //do we need absolute or relative mouse?
simonscott 1:38cd433ff221 21 USBMouseKeyboard key_mouse(ABS_MOUSE);
simonscott 1:38cd433ff221 22 HID_REPORT kbHIDReport;;
simonscott 1:38cd433ff221 23
simonscott 1:38cd433ff221 24 Serial pc(USBTX,USBRX);
simonscott 1:38cd433ff221 25 I2C i2c(D7, D6);
simonscott 1:38cd433ff221 26
simonscott 1:38cd433ff221 27 const int bno055_addr = 0x28 << 1;
simonscott 1:38cd433ff221 28
simonscott 1:38cd433ff221 29 const int BNO055_ID_ADDR = 0x00;
simonscott 1:38cd433ff221 30 const int BNO055_EULER_H_LSB_ADDR = 0x1A;
simonscott 1:38cd433ff221 31 const int BNO055_GRAVITY_DATA_X_LSB_ADDR = 0x2E;
simonscott 1:38cd433ff221 32 const int BNO055_TEMP_ADDR = 0x34;
simonscott 1:38cd433ff221 33 const int BNO055_OPR_MODE_ADDR = 0x3D;
simonscott 1:38cd433ff221 34 const int BNO055_CALIB_STAT_ADDR = 0x35;
simonscott 1:38cd433ff221 35 const int BNO055_SYS_STAT_ADDR = 0x39;
simonscott 1:38cd433ff221 36 const int BNO055_SYS_ERR_ADDR = 0x3A;
simonscott 1:38cd433ff221 37 const int BNO055_AXIS_MAP_CONFIG_ADDR = 0x41;
simonscott 1:38cd433ff221 38 const int BNO055_SYS_TRIGGER_ADDR = 0x3F;
simonscott 1:38cd433ff221 39 const int BNO055_ACC_DATA_X_ADDR = 0x08;
simonscott 1:38cd433ff221 40
simonscott 1:38cd433ff221 41 typedef struct CalibStatus_t
simonscott 1:38cd433ff221 42 {
simonscott 1:38cd433ff221 43 int mag;
simonscott 1:38cd433ff221 44 int acc;
simonscott 1:38cd433ff221 45 int gyr;
simonscott 1:38cd433ff221 46 int sys;
simonscott 1:38cd433ff221 47 } CalibStatus;
simonscott 1:38cd433ff221 48
simonscott 1:38cd433ff221 49 typedef struct Euler_t
simonscott 1:38cd433ff221 50 {
simonscott 1:38cd433ff221 51 float heading;
simonscott 1:38cd433ff221 52 float pitch;
simonscott 1:38cd433ff221 53 float roll;
simonscott 1:38cd433ff221 54 } Euler;
simonscott 1:38cd433ff221 55
simonscott 1:38cd433ff221 56 // The "zero" offset positions
simonscott 1:38cd433ff221 57 short int headingOffset;
simonscott 1:38cd433ff221 58 short int pitchOffset;
simonscott 1:38cd433ff221 59 short int rollOffset;
simonscott 1:38cd433ff221 60
simonscott 1:38cd433ff221 61
simonscott 1:38cd433ff221 62 /**
simonscott 1:38cd433ff221 63 * Function to write to a single 8-bit register
simonscott 1:38cd433ff221 64 */
simonscott 1:38cd433ff221 65 void writeReg(int regAddr, char value)
simonscott 1:38cd433ff221 66 {
simonscott 1:38cd433ff221 67 char wbuf[2];
simonscott 1:38cd433ff221 68 wbuf[0] = regAddr;
simonscott 1:38cd433ff221 69 wbuf[1] = value;
simonscott 1:38cd433ff221 70 i2c.write(bno055_addr, wbuf, 2, false);
simonscott 1:38cd433ff221 71 }
simonscott 1:38cd433ff221 72
simonscott 1:38cd433ff221 73 /**
simonscott 1:38cd433ff221 74 * Function to read from a single 8-bit register
simonscott 1:38cd433ff221 75 */
simonscott 1:38cd433ff221 76 char readReg(int regAddr)
simonscott 1:38cd433ff221 77 {
simonscott 1:38cd433ff221 78 char rwbuf = regAddr;
simonscott 1:38cd433ff221 79 i2c.write(bno055_addr, &rwbuf, 1, false);
simonscott 1:38cd433ff221 80 i2c.read(bno055_addr, &rwbuf, 1, false);
simonscott 1:38cd433ff221 81 return rwbuf;
simonscott 1:38cd433ff221 82 }
simonscott 1:38cd433ff221 83
simonscott 1:38cd433ff221 84 /**
simonscott 1:38cd433ff221 85 * Returns the calibration status of each component
simonscott 1:38cd433ff221 86 */
simonscott 1:38cd433ff221 87 CalibStatus readCalibrationStatus()
simonscott 1:38cd433ff221 88 {
simonscott 1:38cd433ff221 89 CalibStatus status;
simonscott 1:38cd433ff221 90 int regVal = readReg(BNO055_CALIB_STAT_ADDR);
simonscott 1:38cd433ff221 91
simonscott 1:38cd433ff221 92 status.mag = regVal & 0x03;
simonscott 1:38cd433ff221 93 status.acc = (regVal >> 2) & 0x03;
simonscott 1:38cd433ff221 94 status.gyr = (regVal >> 4) & 0x03;
simonscott 1:38cd433ff221 95 status.sys = (regVal >> 6) & 0x03;
simonscott 1:38cd433ff221 96
simonscott 1:38cd433ff221 97 return status;
simonscott 1:38cd433ff221 98 }
simonscott 1:38cd433ff221 99
simonscott 1:38cd433ff221 100
simonscott 1:38cd433ff221 101 /**
simonscott 1:38cd433ff221 102 * Returns true if all the devices are calibrated
simonscott 1:38cd433ff221 103 */
simonscott 1:38cd433ff221 104 bool calibrated()
simonscott 1:38cd433ff221 105 {
simonscott 1:38cd433ff221 106 CalibStatus status = readCalibrationStatus();
simonscott 1:38cd433ff221 107
simonscott 1:38cd433ff221 108 if(status.mag == 3 && status.acc == 3 && status.gyr == 3)
simonscott 1:38cd433ff221 109 return true;
simonscott 1:38cd433ff221 110 else
simonscott 1:38cd433ff221 111 return false;
simonscott 1:38cd433ff221 112 }
simonscott 1:38cd433ff221 113
simonscott 1:38cd433ff221 114
simonscott 1:38cd433ff221 115 /**
simonscott 1:38cd433ff221 116 * Checks that there are no errors on the accelerometer
simonscott 1:38cd433ff221 117 */
simonscott 1:38cd433ff221 118 bool bno055Healthy()
simonscott 1:38cd433ff221 119 {
simonscott 1:38cd433ff221 120 int sys_error = readReg(BNO055_SYS_ERR_ADDR);
simonscott 1:38cd433ff221 121 wait(0.001);
simonscott 1:38cd433ff221 122 int sys_stat = readReg(BNO055_SYS_STAT_ADDR);
simonscott 1:38cd433ff221 123 wait(0.001);
simonscott 1:38cd433ff221 124
simonscott 1:38cd433ff221 125 if(sys_error == 0 && sys_stat == 5)
simonscott 1:38cd433ff221 126 return true;
simonscott 1:38cd433ff221 127 else {
simonscott 1:38cd433ff221 128 //pc.printf("SYS_ERR: %d SYS_STAT: %d\r\n", sys_error, sys_stat);
simonscott 1:38cd433ff221 129 return false;
simonscott 1:38cd433ff221 130 }
simonscott 1:38cd433ff221 131 }
simonscott 1:38cd433ff221 132
simonscott 1:38cd433ff221 133
simonscott 1:38cd433ff221 134 /**
simonscott 1:38cd433ff221 135 * Configure and initialize the BNO055
simonscott 1:38cd433ff221 136 */
simonscott 1:38cd433ff221 137 bool initBNO055()
simonscott 1:38cd433ff221 138 {
simonscott 1:38cd433ff221 139 unsigned char regVal;
simonscott 1:38cd433ff221 140 i2c.frequency(400000);
simonscott 1:38cd433ff221 141 bool startupPass = true;
simonscott 1:38cd433ff221 142
simonscott 1:38cd433ff221 143 // Do some basic power-up tests
simonscott 1:38cd433ff221 144 regVal = readReg(BNO055_ID_ADDR);
simonscott 1:38cd433ff221 145 if(regVal == 0xA0)
simonscott 1:38cd433ff221 146 pc.printf("BNO055 successfully detected!\r\n");
simonscott 1:38cd433ff221 147 else {
simonscott 1:38cd433ff221 148 pc.printf("ERROR: no BNO055 detected\r\n");
simonscott 1:38cd433ff221 149 startupPass = false;
simonscott 1:38cd433ff221 150 }
simonscott 1:38cd433ff221 151
simonscott 1:38cd433ff221 152 regVal = readReg(BNO055_TEMP_ADDR);
simonscott 1:38cd433ff221 153 pc.printf("Chip temperature is: %d C\r\n", regVal);
simonscott 1:38cd433ff221 154
simonscott 1:38cd433ff221 155 if(regVal == 0)
simonscott 1:38cd433ff221 156 startupPass = false;
simonscott 1:38cd433ff221 157
simonscott 1:38cd433ff221 158 // Change mode to CONFIG
simonscott 1:38cd433ff221 159 writeReg(BNO055_OPR_MODE_ADDR, 0x00);
simonscott 1:38cd433ff221 160 wait(0.2);
simonscott 1:38cd433ff221 161
simonscott 1:38cd433ff221 162 regVal = readReg(BNO055_OPR_MODE_ADDR);
simonscott 1:38cd433ff221 163 pc.printf("Change to mode: %d\r\n", regVal);
simonscott 1:38cd433ff221 164 wait(0.1);
simonscott 1:38cd433ff221 165
simonscott 1:38cd433ff221 166 // Remap axes
simonscott 1:38cd433ff221 167 writeReg(BNO055_AXIS_MAP_CONFIG_ADDR, 0x06); // b00_00_01_10
simonscott 1:38cd433ff221 168 wait(0.1);
simonscott 1:38cd433ff221 169
simonscott 1:38cd433ff221 170 // Set to external crystal
simonscott 1:38cd433ff221 171 writeReg(BNO055_SYS_TRIGGER_ADDR, 0x80);
simonscott 1:38cd433ff221 172 wait(0.2);
simonscott 1:38cd433ff221 173
simonscott 1:38cd433ff221 174 // Change mode to NDOF
simonscott 1:38cd433ff221 175 writeReg(BNO055_OPR_MODE_ADDR, 0x0C);
simonscott 1:38cd433ff221 176 wait(0.2);
simonscott 1:38cd433ff221 177
simonscott 1:38cd433ff221 178 regVal = readReg(BNO055_OPR_MODE_ADDR);
simonscott 1:38cd433ff221 179 pc.printf("Change to mode: %d\r\n", regVal);
simonscott 1:38cd433ff221 180 wait(0.1);
simonscott 1:38cd433ff221 181
simonscott 1:38cd433ff221 182 return startupPass;
simonscott 1:38cd433ff221 183 }
simonscott 1:38cd433ff221 184
simonscott 1:38cd433ff221 185 /**
simonscott 1:38cd433ff221 186 * Sets the current accelerometer position as the zero position.
simonscott 1:38cd433ff221 187 */
simonscott 1:38cd433ff221 188 void setZeroPosition()
simonscott 1:38cd433ff221 189 {
simonscott 1:38cd433ff221 190 char buf[16];
simonscott 1:38cd433ff221 191
simonscott 1:38cd433ff221 192 // Read the current euler angles and set them as the zero position
simonscott 1:38cd433ff221 193 buf[0] = BNO055_EULER_H_LSB_ADDR;
simonscott 1:38cd433ff221 194 i2c.write(bno055_addr, buf, 1, false);
simonscott 1:38cd433ff221 195 i2c.read(bno055_addr, buf, 6, false);
simonscott 1:38cd433ff221 196
simonscott 1:38cd433ff221 197 headingOffset = buf[0] + (buf[1] << 8);
simonscott 1:38cd433ff221 198 rollOffset = buf[2] + (buf[3] << 8);
simonscott 1:38cd433ff221 199 pitchOffset = buf[4] + (buf[5] << 8);
simonscott 1:38cd433ff221 200 }
simonscott 1:38cd433ff221 201
simonscott 1:38cd433ff221 202
simonscott 1:38cd433ff221 203 /**
simonscott 1:38cd433ff221 204 * Sets the current accelerometer position as the zero position.
simonscott 1:38cd433ff221 205 */
simonscott 1:38cd433ff221 206 void setZeroHeading()
simonscott 1:38cd433ff221 207 {
simonscott 1:38cd433ff221 208 char buf[16];
simonscott 1:38cd433ff221 209
simonscott 1:38cd433ff221 210 // Read the current euler angles and set them as the zero position
simonscott 1:38cd433ff221 211 buf[0] = BNO055_EULER_H_LSB_ADDR;
simonscott 1:38cd433ff221 212 i2c.write(bno055_addr, buf, 1, false);
simonscott 1:38cd433ff221 213 i2c.read(bno055_addr, buf, 6, false);
simonscott 1:38cd433ff221 214
simonscott 1:38cd433ff221 215 headingOffset = buf[0] + (buf[1] << 8);
simonscott 1:38cd433ff221 216 }
simonscott 1:38cd433ff221 217
simonscott 1:38cd433ff221 218
simonscott 1:38cd433ff221 219 /**
simonscott 1:38cd433ff221 220 * Reads the Euler angles, zeroed out
simonscott 1:38cd433ff221 221 */
simonscott 1:38cd433ff221 222 Euler getEulerAngles()
simonscott 1:38cd433ff221 223 {
simonscott 1:38cd433ff221 224 char buf[16];
simonscott 1:38cd433ff221 225 Euler e;
simonscott 1:38cd433ff221 226
simonscott 1:38cd433ff221 227 // Read in the Euler angles
simonscott 1:38cd433ff221 228 buf[0] = BNO055_EULER_H_LSB_ADDR;
simonscott 1:38cd433ff221 229 i2c.write(bno055_addr, buf, 1, false);
simonscott 1:38cd433ff221 230 i2c.read(bno055_addr, buf, 6, false);
simonscott 1:38cd433ff221 231
simonscott 1:38cd433ff221 232 short int euler_head = buf[0] + (buf[1] << 8);
simonscott 1:38cd433ff221 233 short int euler_roll = buf[2] + (buf[3] << 8);
simonscott 1:38cd433ff221 234 short int euler_pitch = buf[4] + (buf[5] << 8);
simonscott 1:38cd433ff221 235
simonscott 1:38cd433ff221 236 e.heading = ((int)euler_head - (int)headingOffset) / 16.0;
simonscott 1:38cd433ff221 237 e.roll = ((int)euler_roll - (int)rollOffset) / 16.0;
simonscott 1:38cd433ff221 238 e.pitch = ((int)euler_pitch - (int)pitchOffset) / 16.0;
simonscott 1:38cd433ff221 239
simonscott 1:38cd433ff221 240 if(e.pitch > 90 || e.pitch < -90)
simonscott 1:38cd433ff221 241 e.pitch = 0;
simonscott 1:38cd433ff221 242
simonscott 1:38cd433ff221 243 return e;
simonscott 1:38cd433ff221 244 }
simonscott 1:38cd433ff221 245
simonscott 1:38cd433ff221 246
simonscott 1:38cd433ff221 247 /***** Functions to vibrate the motor (non-blocking call) *****/
simonscott 1:38cd433ff221 248 void vibrateOff()
simonscott 1:38cd433ff221 249 {
simonscott 1:38cd433ff221 250 vibrate = 0;
simonscott 1:38cd433ff221 251 }
simonscott 1:38cd433ff221 252
simonscott 1:38cd433ff221 253 void vibrateMotor()
simonscott 1:38cd433ff221 254 {
simonscott 1:38cd433ff221 255 vibrate = 1;
simonscott 1:38cd433ff221 256 vibrateTimeout.attach(&vibrateOff, 0.1);
simonscott 1:38cd433ff221 257 }
simonscott 1:38cd433ff221 258
simonscott 1:38cd433ff221 259
simonscott 1:38cd433ff221 260
simonscott 1:38cd433ff221 261
simonscott 1:38cd433ff221 262 int main() {
simonscott 1:38cd433ff221 263
simonscott 1:38cd433ff221 264 uint16_t x_center = (X_MAX_ABS - X_MIN_ABS)/2;
simonscott 1:38cd433ff221 265 uint16_t y_center = (Y_MAX_ABS - Y_MIN_ABS)/2;
simonscott 1:38cd433ff221 266 uint16_t x_screen = 0;
simonscott 1:38cd433ff221 267 uint16_t y_screen = 0;
simonscott 1:38cd433ff221 268
simonscott 1:38cd433ff221 269 int loop_counter = 0;
simonscott 1:38cd433ff221 270 int shake_counter = 0;
simonscott 1:38cd433ff221 271
simonscott 1:38cd433ff221 272
simonscott 1:38cd433ff221 273 //uint32_t x_origin = x_center;
simonscott 1:38cd433ff221 274 //uint32_t y_origin = y_center;
simonscott 1:38cd433ff221 275 //uint32_t radius = 5000;
simonscott 1:38cd433ff221 276 //uint32_t angle = 0;
simonscott 1:38cd433ff221 277
simonscott 1:38cd433ff221 278 redLED = 0;
simonscott 1:38cd433ff221 279 bool startupPassed;
simonscott 1:38cd433ff221 280 Euler e;
simonscott 1:38cd433ff221 281 bool down;
simonscott 1:38cd433ff221 282 CalibStatus calStat;
simonscott 1:38cd433ff221 283
simonscott 1:38cd433ff221 284 // Record old state of buttons
simonscott 1:38cd433ff221 285 bool triggerPressed = false;
simonscott 1:38cd433ff221 286 bool movePressed = false;
simonscott 1:38cd433ff221 287
simonscott 1:38cd433ff221 288 // USB HID report to send move up/down
simonscott 1:38cd433ff221 289 kbHIDReport.length = 4;
simonscott 1:38cd433ff221 290 kbHIDReport.data[0] = 1; // USB ID
simonscott 1:38cd433ff221 291 kbHIDReport.data[1] = 0; // modifier key
simonscott 1:38cd433ff221 292 kbHIDReport.data[2] = 0; // don't know
simonscott 1:38cd433ff221 293
simonscott 1:38cd433ff221 294 // Initialize
simonscott 1:38cd433ff221 295 pc.baud(115200);
simonscott 1:38cd433ff221 296 trigger.mode(PullUp);
simonscott 1:38cd433ff221 297 move.mode(PullUp);
simonscott 1:38cd433ff221 298 door.mode(PullUp);
simonscott 1:38cd433ff221 299 centerBut.mode(PullUp);
simonscott 1:38cd433ff221 300 wait(0.8);
simonscott 1:38cd433ff221 301 startupPassed = initBNO055(); // Note: set LED to RED if this fails
simonscott 1:38cd433ff221 302
simonscott 1:38cd433ff221 303 // Wait until calibration passes
simonscott 1:38cd433ff221 304
simonscott 1:38cd433ff221 305 while(!calibrated()){
simonscott 1:38cd433ff221 306 wait(0.1);
simonscott 1:38cd433ff221 307 calStat = readCalibrationStatus();
simonscott 1:38cd433ff221 308 printf("MAG: %d ACC: %d GYR: %d SYS: %d\r\n", calStat.mag, calStat.acc, calStat.gyr, calStat.sys);
simonscott 1:38cd433ff221 309 wait(0.5);
simonscott 1:38cd433ff221 310 }
simonscott 1:38cd433ff221 311 redLED = 1;
simonscott 1:38cd433ff221 312 greenLED = 0;
simonscott 1:38cd433ff221 313
simonscott 1:38cd433ff221 314 pc.printf("Board fully calibrated!\r\n");
simonscott 1:38cd433ff221 315
simonscott 1:38cd433ff221 316 // Wait until user hits the trigger. Then zero out the readings
simonscott 1:38cd433ff221 317 while(trigger.read() == 1) {
simonscott 1:38cd433ff221 318 wait(0.01);
simonscott 1:38cd433ff221 319 }
simonscott 1:38cd433ff221 320 setZeroPosition();
simonscott 1:38cd433ff221 321
simonscott 1:38cd433ff221 322 // Read orientation values
simonscott 1:38cd433ff221 323 while(true)
simonscott 1:38cd433ff221 324 {
simonscott 1:38cd433ff221 325 // Make sure that there are no errors
simonscott 1:38cd433ff221 326 if(!bno055Healthy())
simonscott 1:38cd433ff221 327 {
simonscott 1:38cd433ff221 328 //wait(0.1);
simonscott 1:38cd433ff221 329 //calStat = readCalibrationStatus();
simonscott 1:38cd433ff221 330 //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);
simonscott 1:38cd433ff221 331 pc.printf("ERROR: BNO055 has an error/status problem!!!\r\n");
simonscott 1:38cd433ff221 332 }
simonscott 1:38cd433ff221 333
simonscott 1:38cd433ff221 334 // Read in the Euler angles
simonscott 1:38cd433ff221 335 e = getEulerAngles();
simonscott 1:38cd433ff221 336 wait(0.001);
simonscott 1:38cd433ff221 337
simonscott 1:38cd433ff221 338 // Read in the calibration status
simonscott 1:38cd433ff221 339 calStat = readCalibrationStatus();
simonscott 1:38cd433ff221 340 wait(0.001);
simonscott 1:38cd433ff221 341
simonscott 1:38cd433ff221 342 // LED red if not calibrated else green
simonscott 1:38cd433ff221 343 if(!calibrated())
simonscott 1:38cd433ff221 344 {
simonscott 1:38cd433ff221 345 redLED = 0;
simonscott 1:38cd433ff221 346 greenLED = 1;
simonscott 1:38cd433ff221 347 }
simonscott 1:38cd433ff221 348 else
simonscott 1:38cd433ff221 349 {
simonscott 1:38cd433ff221 350 redLED = 1;
simonscott 1:38cd433ff221 351 greenLED = 0;
simonscott 1:38cd433ff221 352 }
simonscott 1:38cd433ff221 353
simonscott 1:38cd433ff221 354 /*
simonscott 1:38cd433ff221 355 // Check if device is pointing down
simonscott 1:38cd433ff221 356 //down = (e.pitch < -70);
simonscott 1:38cd433ff221 357
simonscott 1:38cd433ff221 358 wait(0.01);
simonscott 1:38cd433ff221 359 char buf[16];
simonscott 1:38cd433ff221 360 buf[0] = BNO055_ACC_DATA_X_ADDR;
simonscott 1:38cd433ff221 361 i2c.write(bno055_addr, buf, 1, false);
simonscott 1:38cd433ff221 362 i2c.read(bno055_addr, buf, 6, false);
simonscott 1:38cd433ff221 363
simonscott 1:38cd433ff221 364 short int acc_z = buf[4] + (buf[5] << 8);
simonscott 1:38cd433ff221 365 wait(0.01);
simonscott 1:38cd433ff221 366
simonscott 1:38cd433ff221 367 if(acc_z < -1000 || acc_z > 1000)
simonscott 1:38cd433ff221 368 {
simonscott 1:38cd433ff221 369 shake_counter++;
simonscott 1:38cd433ff221 370 loop_counter++;
simonscott 1:38cd433ff221 371 }
simonscott 1:38cd433ff221 372 else if (loop_counter > 0)
simonscott 1:38cd433ff221 373 loop_counter ++;
simonscott 1:38cd433ff221 374
simonscott 1:38cd433ff221 375 if(shake_counter > 2 && loop_counter < 20) {
simonscott 1:38cd433ff221 376 key_mouse.scroll(-1);
simonscott 1:38cd433ff221 377 wait(1.5);
simonscott 1:38cd433ff221 378 shake_counter = 0;
simonscott 1:38cd433ff221 379 loop_counter = 0;
simonscott 1:38cd433ff221 380 }
simonscott 1:38cd433ff221 381 else if(loop_counter == 20) {
simonscott 1:38cd433ff221 382 shake_counter = 0;
simonscott 1:38cd433ff221 383 loop_counter = 0;
simonscott 1:38cd433ff221 384 }
simonscott 1:38cd433ff221 385 */
simonscott 1:38cd433ff221 386 /*
simonscott 1:38cd433ff221 387 //if it is down, then change device
simonscott 1:38cd433ff221 388 if(down){
simonscott 1:38cd433ff221 389 key_mouse.scroll(-1);
simonscott 1:38cd433ff221 390
simonscott 1:38cd433ff221 391 // Wait until up
simonscott 1:38cd433ff221 392 do {
simonscott 1:38cd433ff221 393 wait(0.01);
simonscott 1:38cd433ff221 394 e = getEulerAngles();
simonscott 1:38cd433ff221 395 wait(0.01);
simonscott 1:38cd433ff221 396 } while(e.pitch < -20);
simonscott 1:38cd433ff221 397
simonscott 1:38cd433ff221 398 wait(2);
simonscott 1:38cd433ff221 399
simonscott 1:38cd433ff221 400 // Re-center heading
simonscott 1:38cd433ff221 401 //setZeroHeading();
simonscott 1:38cd433ff221 402 //wait(0.01);
simonscott 1:38cd433ff221 403 }
simonscott 1:38cd433ff221 404 */
simonscott 1:38cd433ff221 405 // If trigger state changed
simonscott 1:38cd433ff221 406 if (!trigger.read() != triggerPressed)
simonscott 1:38cd433ff221 407 {
simonscott 1:38cd433ff221 408 // If trigger pressed
simonscott 1:38cd433ff221 409 if(!trigger.read())
simonscott 1:38cd433ff221 410 {
simonscott 1:38cd433ff221 411 //key_mouse.click(MOUSE_LEFT);
simonscott 1:38cd433ff221 412 //key_mouse.keyCode('d');
simonscott 1:38cd433ff221 413 kbHIDReport.data[3] = 0x07; // UP arrow
simonscott 1:38cd433ff221 414 vibrateMotor();
simonscott 1:38cd433ff221 415 triggerPressed = true;
simonscott 1:38cd433ff221 416 }
simonscott 1:38cd433ff221 417
simonscott 1:38cd433ff221 418 else {
simonscott 1:38cd433ff221 419 triggerPressed = false;
simonscott 1:38cd433ff221 420 kbHIDReport.data[3] = 0x00; // UP arrow
simonscott 1:38cd433ff221 421 }
simonscott 1:38cd433ff221 422 key_mouse.sendNB(&kbHIDReport);
simonscott 1:38cd433ff221 423 }
simonscott 1:38cd433ff221 424
simonscott 1:38cd433ff221 425 if (!door.read()){
simonscott 1:38cd433ff221 426 int counter = 0;
simonscott 1:38cd433ff221 427 while(counter<50){
simonscott 1:38cd433ff221 428 key_mouse.keyCode(' ');
simonscott 1:38cd433ff221 429 counter++;
simonscott 1:38cd433ff221 430 wait(0.001);
simonscott 1:38cd433ff221 431 }
simonscott 1:38cd433ff221 432 wait(0.2);
simonscott 1:38cd433ff221 433 }
simonscott 1:38cd433ff221 434
simonscott 1:38cd433ff221 435 if(!centerBut.read())
simonscott 1:38cd433ff221 436 {
simonscott 1:38cd433ff221 437 wait(0.01);
simonscott 1:38cd433ff221 438 setZeroPosition();
simonscott 1:38cd433ff221 439 wait(0.1);
simonscott 1:38cd433ff221 440 }
simonscott 1:38cd433ff221 441
simonscott 1:38cd433ff221 442 // If move button state changed
simonscott 1:38cd433ff221 443 if (!move.read() != movePressed)
simonscott 1:38cd433ff221 444 {
simonscott 1:38cd433ff221 445 // If move pressed
simonscott 1:38cd433ff221 446 if(!move.read()) {
simonscott 1:38cd433ff221 447 kbHIDReport.data[3] = 0x52; // UP arrow
simonscott 1:38cd433ff221 448 movePressed = true;
simonscott 1:38cd433ff221 449 }
simonscott 1:38cd433ff221 450
simonscott 1:38cd433ff221 451 else {
simonscott 1:38cd433ff221 452 kbHIDReport.data[3] = 0x00; // no press
simonscott 1:38cd433ff221 453 movePressed = false;
simonscott 1:38cd433ff221 454 }
simonscott 1:38cd433ff221 455
simonscott 1:38cd433ff221 456 key_mouse.sendNB(&kbHIDReport);
simonscott 1:38cd433ff221 457 }
simonscott 1:38cd433ff221 458
simonscott 1:38cd433ff221 459
simonscott 1:38cd433ff221 460 // move limits
simonscott 1:38cd433ff221 461 float heading_limited;
simonscott 1:38cd433ff221 462 if(e.heading > 90)
simonscott 1:38cd433ff221 463 heading_limited = 90;
simonscott 1:38cd433ff221 464 else if(e.heading < -90)
simonscott 1:38cd433ff221 465 heading_limited = -90;
simonscott 1:38cd433ff221 466 else if(e.heading < 2 && e.heading > -2)
simonscott 1:38cd433ff221 467 heading_limited = 0;
simonscott 1:38cd433ff221 468 //else if (e.pitch < -30)
simonscott 1:38cd433ff221 469 // heading_limited = 0;
simonscott 1:38cd433ff221 470 else if(e.heading > -20 && e.heading < 20)
simonscott 1:38cd433ff221 471 heading_limited = 16 * e.heading;
simonscott 1:38cd433ff221 472 else
simonscott 1:38cd433ff221 473 heading_limited = 0.8 * e.heading * abs(e.heading);
simonscott 1:38cd433ff221 474
simonscott 1:38cd433ff221 475
simonscott 1:38cd433ff221 476
simonscott 1:38cd433ff221 477 //moving the mouse now
simonscott 1:38cd433ff221 478 x_screen = x_center + heading_limited;
simonscott 1:38cd433ff221 479 //x_screen = x_center+(e.heading/180*(X_MAX_ABS-x_center)); //45 was too sensitive
simonscott 1:38cd433ff221 480 y_screen = y_center;
simonscott 1:38cd433ff221 481
simonscott 1:38cd433ff221 482 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,
simonscott 1:38cd433ff221 483 down, calStat.mag, calStat.acc, calStat.gyr, calStat.sys);
simonscott 1:38cd433ff221 484
simonscott 1:38cd433ff221 485 key_mouse.move(x_screen, y_screen);
simonscott 1:38cd433ff221 486 wait(0.02);
simonscott 1:38cd433ff221 487 }
simonscott 1:38cd433ff221 488 }