Ben, Simon, Inez IDD / Mbed 2 deprecated BNO055_reader

Program to simulate gun for doom using BNO055 sensor

Dependencies:   USBDevice mbed DebounceIn

main.cpp@1:38cd433ff221, 2015-09-20 (annotated)

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

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