Andrew Lindsay / Mbed 2 deprecated Wi-Go-MagnetometerTest

Quick test of the Wi-Go Magnetometer

Dependencies:   TSI mbed MAG3110

main.cpp@2:c19e63728e2e, 2013-05-18 (annotated)

Committer:
SomeRandomBloke
Date:
Sat May 18 10:00:03 2013 +0000
Revision:
2:c19e63728e2e
Parent:
1:d45fc466a46e
Child:
3:945e32be0448
initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SomeRandomBloke 2:c19e63728e2e 1 /**
SomeRandomBloke 2:c19e63728e2e 2 * Simple MAG3110 test/demo program, based on various bits of code found.
SomeRandomBloke 2:c19e63728e2e 3 * Updated for Mbed FRDM-KL25Z and Avnet Wi-Go module.
SomeRandomBloke 2:c19e63728e2e 4 *
SomeRandomBloke 2:c19e63728e2e 5 * Operation:
SomeRandomBloke 2:c19e63728e2e 6 * 1. On startup Red LED flashes indicating calibration mode entered
SomeRandomBloke 2:c19e63728e2e 7 * 2. Slide finger along capacitive sensor and release
SomeRandomBloke 2:c19e63728e2e 8 * 3. Green LED flashes indicating calibration mode.
SomeRandomBloke 2:c19e63728e2e 9 * 4. Rotate board once in horizontal plane
SomeRandomBloke 2:c19e63728e2e 10 * 5. Tap and release capacitive sensor. Board now calibrated with min/max values
SomeRandomBloke 2:c19e63728e2e 11 * 6. LEDs now off. Rotate board. When Blue LED lights the bottom of the board is
SomeRandomBloke 2:c19e63728e2e 12 * pointing to approximately North (+/- 22.5')
SomeRandomBloke 2:c19e63728e2e 13 *
SomeRandomBloke 2:c19e63728e2e 14 * If USB cable connected then you would see debug messages and other headings displayed, e.g. N, NE, E, SE, S, SW, W and NW.
SomeRandomBloke 2:c19e63728e2e 15 *
SomeRandomBloke 2:c19e63728e2e 16 * Also displays register values when starting.
SomeRandomBloke 2:c19e63728e2e 17 *
SomeRandomBloke 2:c19e63728e2e 18 * Ideally the calibration settings would be stored in flash/eeprom and retrieves each time.
SomeRandomBloke 2:c19e63728e2e 19 *
SomeRandomBloke 2:c19e63728e2e 20 * By Andrew D. Lindsay, @AndrewDLindsay
SomeRandomBloke 2:c19e63728e2e 21 *
SomeRandomBloke 2:c19e63728e2e 22 *
SomeRandomBloke 2:c19e63728e2e 23 *
SomeRandomBloke 2:c19e63728e2e 24 */
SomeRandomBloke 2:c19e63728e2e 25
SomeRandomBloke 0:26bfff579f01 26 #include "mbed.h"
SomeRandomBloke 1:d45fc466a46e 27 #include "TSISensor.h"
SomeRandomBloke 0:26bfff579f01 28 #include "math.h"
SomeRandomBloke 0:26bfff579f01 29
SomeRandomBloke 2:c19e63728e2e 30 #define MAG_ADDR 0x1D
SomeRandomBloke 0:26bfff579f01 31
SomeRandomBloke 2:c19e63728e2e 32 // define registers
SomeRandomBloke 0:26bfff579f01 33 #define MAG_DR_STATUS 0x00
SomeRandomBloke 0:26bfff579f01 34 #define MAG_OUT_X_MSB 0x01
SomeRandomBloke 0:26bfff579f01 35 #define MAG_OUT_X_LSB 0x02
SomeRandomBloke 0:26bfff579f01 36 #define MAG_OUT_Y_MSB 0x03
SomeRandomBloke 0:26bfff579f01 37 #define MAG_OUT_Y_LSB 0x04
SomeRandomBloke 0:26bfff579f01 38 #define MAG_OUT_Z_MSB 0x05
SomeRandomBloke 0:26bfff579f01 39 #define MAG_OUT_Z_LSB 0x06
SomeRandomBloke 0:26bfff579f01 40 #define MAG_WHO_AM_I 0x07
SomeRandomBloke 0:26bfff579f01 41 #define MAG_SYSMOD 0x08
SomeRandomBloke 0:26bfff579f01 42 #define MAG_OFF_X_MSB 0x09
SomeRandomBloke 0:26bfff579f01 43 #define MAG_OFF_X_LSB 0x0A
SomeRandomBloke 0:26bfff579f01 44 #define MAG_OFF_Y_MSB 0x0B
SomeRandomBloke 0:26bfff579f01 45 #define MAG_OFF_Y_LSB 0x0C
SomeRandomBloke 0:26bfff579f01 46 #define MAG_OFF_Z_MSB 0x0D
SomeRandomBloke 0:26bfff579f01 47 #define MAG_OFF_Z_LSB 0x0E
SomeRandomBloke 0:26bfff579f01 48 #define MAG_DIE_TEMP 0x0F
SomeRandomBloke 0:26bfff579f01 49 #define MAG_CTRL_REG1 0x10
SomeRandomBloke 0:26bfff579f01 50 #define MAG_CTRL_REG2 0x11
SomeRandomBloke 0:26bfff579f01 51
SomeRandomBloke 0:26bfff579f01 52 // what should WHO_AM_I return?
SomeRandomBloke 0:26bfff579f01 53 #define MAG_3110_WHO_AM_I_VALUE 0xC4
SomeRandomBloke 0:26bfff579f01 54
SomeRandomBloke 0:26bfff579f01 55
SomeRandomBloke 0:26bfff579f01 56 // Fields in registers
SomeRandomBloke 0:26bfff579f01 57 // CTRL_REG1: dr2,dr1,dr0 os1,os0 fr tm ac
SomeRandomBloke 0:26bfff579f01 58
SomeRandomBloke 0:26bfff579f01 59 // Sampling rate from 80Hz down to 0.625Hz
SomeRandomBloke 0:26bfff579f01 60 #define MAG_3110_SAMPLE80 0
SomeRandomBloke 0:26bfff579f01 61 #define MAG_3110_SAMPLE40 0x20
SomeRandomBloke 0:26bfff579f01 62 #define MAG_3110_SAMPLE20 0x40
SomeRandomBloke 0:26bfff579f01 63 #define MAG_3110_SAMPLE10 0x60
SomeRandomBloke 0:26bfff579f01 64 #define MAG_3110_SAMPLE5 0x80
SomeRandomBloke 0:26bfff579f01 65 #define MAG_3110_SAMPLE2_5 0xA0
SomeRandomBloke 0:26bfff579f01 66 #define MAG_3110_SAMPLE1_25 0xC0
SomeRandomBloke 0:26bfff579f01 67 #define MAG_3110_SAMPLE0_625 0xE0
SomeRandomBloke 0:26bfff579f01 68
SomeRandomBloke 0:26bfff579f01 69 // How many samples to average (lowers data rate)
SomeRandomBloke 0:26bfff579f01 70 #define MAG_3110_OVERSAMPLE1 0
SomeRandomBloke 0:26bfff579f01 71 #define MAG_3110_OVERSAMPLE2 0x08
SomeRandomBloke 0:26bfff579f01 72 #define MAG_3110_OVERSAMPLE3 0x10
SomeRandomBloke 0:26bfff579f01 73 #define MAG_3110_OVERSAMPLE4 0x18
SomeRandomBloke 0:26bfff579f01 74
SomeRandomBloke 0:26bfff579f01 75 // read only 1 byte per axis
SomeRandomBloke 0:26bfff579f01 76 #define MAG_3110_FASTREAD 0x04
SomeRandomBloke 0:26bfff579f01 77 // do one measurement (even if in standby mode)
SomeRandomBloke 0:26bfff579f01 78 #define MAG_3110_TRIGGER 0x02
SomeRandomBloke 0:26bfff579f01 79 // put in active mode
SomeRandomBloke 0:26bfff579f01 80 #define MAG_3110_ACTIVE 0x01
SomeRandomBloke 0:26bfff579f01 81
SomeRandomBloke 0:26bfff579f01 82 // CTRL_REG2: AUTO_MRST_EN _ RAW MAG_RST _ _ _ _ _
SomeRandomBloke 0:26bfff579f01 83 // reset sensor after each reading
SomeRandomBloke 0:26bfff579f01 84 #define MAG_3110_AUTO_MRST_EN 0x80
SomeRandomBloke 0:26bfff579f01 85 // don't subtract user offsets
SomeRandomBloke 0:26bfff579f01 86 #define MAG_3110_RAW 0x20
SomeRandomBloke 0:26bfff579f01 87 // reset magnetic sensor after too-large field
SomeRandomBloke 0:26bfff579f01 88 #define MAG_3110_MAG_RST 0x10
SomeRandomBloke 0:26bfff579f01 89
SomeRandomBloke 0:26bfff579f01 90 // DR_STATUS Register ZYXOW ZOW YOW XOW ZYXDR ZDR YDR XDR
SomeRandomBloke 0:26bfff579f01 91 #define MAG_3110_ZYXDR 0x08
SomeRandomBloke 0:26bfff579f01 92
SomeRandomBloke 0:26bfff579f01 93
SomeRandomBloke 2:c19e63728e2e 94 #define PI 3.14159265359
SomeRandomBloke 2:c19e63728e2e 95 #define ON 0
SomeRandomBloke 2:c19e63728e2e 96 #define OFF 1
SomeRandomBloke 0:26bfff579f01 97
SomeRandomBloke 2:c19e63728e2e 98 // Some LEDs for showing status
SomeRandomBloke 0:26bfff579f01 99 DigitalOut redLed(LED_RED);
SomeRandomBloke 0:26bfff579f01 100 DigitalOut greenLed(LED_GREEN);
SomeRandomBloke 0:26bfff579f01 101 DigitalOut blueLed(LED_BLUE);
SomeRandomBloke 0:26bfff579f01 102
SomeRandomBloke 2:c19e63728e2e 103 // Slide sensor acts as a button
SomeRandomBloke 1:d45fc466a46e 104 TSISensor tsi;
SomeRandomBloke 2:c19e63728e2e 105
SomeRandomBloke 2:c19e63728e2e 106 // I2C used to communicate with sensor
SomeRandomBloke 0:26bfff579f01 107 I2C i2c(PTE0, PTE1);
SomeRandomBloke 0:26bfff579f01 108
SomeRandomBloke 0:26bfff579f01 109 int avgX, avgY, newX, tempXmin, tempXmax, newY, tempYmin, tempYmax;
SomeRandomBloke 2:c19e63728e2e 110 // Ideally these would be saved in eeprom/flash
SomeRandomBloke 0:26bfff579f01 111 struct settings_t {
SomeRandomBloke 0:26bfff579f01 112 long maxX, minX, maxY, minY;
SomeRandomBloke 0:26bfff579f01 113 }
SomeRandomBloke 0:26bfff579f01 114 settings;
SomeRandomBloke 0:26bfff579f01 115
SomeRandomBloke 0:26bfff579f01 116 const int addr = MAG_ADDR;
SomeRandomBloke 0:26bfff579f01 117
SomeRandomBloke 0:26bfff579f01 118 // Read a single byte form 8 bit register, return as int
SomeRandomBloke 0:26bfff579f01 119 int readReg(char regAddr)
SomeRandomBloke 0:26bfff579f01 120 {
SomeRandomBloke 0:26bfff579f01 121 char cmd[1];
SomeRandomBloke 0:26bfff579f01 122
SomeRandomBloke 0:26bfff579f01 123 cmd[0] = regAddr;
SomeRandomBloke 0:26bfff579f01 124 i2c.write(addr, cmd, 1);
SomeRandomBloke 0:26bfff579f01 125
SomeRandomBloke 0:26bfff579f01 126 cmd[0] = 0x00;
SomeRandomBloke 0:26bfff579f01 127 i2c.read(addr, cmd, 1);
SomeRandomBloke 0:26bfff579f01 128 return (int)( cmd[0]);
SomeRandomBloke 0:26bfff579f01 129 }
SomeRandomBloke 0:26bfff579f01 130
SomeRandomBloke 0:26bfff579f01 131
SomeRandomBloke 0:26bfff579f01 132 // read a register per, pass first reg value, reading 2 bytes increments register
SomeRandomBloke 0:26bfff579f01 133 // Reads MSB first then LSB
SomeRandomBloke 0:26bfff579f01 134 int readVal(char regAddr)
SomeRandomBloke 0:26bfff579f01 135 {
SomeRandomBloke 0:26bfff579f01 136 char cmd[2];
SomeRandomBloke 0:26bfff579f01 137
SomeRandomBloke 0:26bfff579f01 138 cmd[0] = regAddr;
SomeRandomBloke 0:26bfff579f01 139 i2c.write(addr, cmd, 1);
SomeRandomBloke 0:26bfff579f01 140
SomeRandomBloke 0:26bfff579f01 141 cmd[0] = 0x00;
SomeRandomBloke 0:26bfff579f01 142 cmd[1] = 0x00;
SomeRandomBloke 0:26bfff579f01 143 i2c.read(addr, cmd, 2);
SomeRandomBloke 0:26bfff579f01 144 return (int)( (cmd[1]|(cmd[0] << 8))); //concatenate the MSB and LSB
SomeRandomBloke 0:26bfff579f01 145 }
SomeRandomBloke 0:26bfff579f01 146
SomeRandomBloke 2:c19e63728e2e 147
SomeRandomBloke 2:c19e63728e2e 148 void initMag() {
SomeRandomBloke 2:c19e63728e2e 149 char cmd[2];
SomeRandomBloke 0:26bfff579f01 150
SomeRandomBloke 2:c19e63728e2e 151 cmd[0] = MAG_CTRL_REG2;
SomeRandomBloke 2:c19e63728e2e 152 cmd[1] = 0x80;
SomeRandomBloke 2:c19e63728e2e 153 i2c.write(addr, cmd, 2);
SomeRandomBloke 2:c19e63728e2e 154
SomeRandomBloke 2:c19e63728e2e 155 cmd[0] = MAG_CTRL_REG1;
SomeRandomBloke 2:c19e63728e2e 156 cmd[1] = MAG_3110_SAMPLE80+MAG_3110_OVERSAMPLE2+MAG_3110_ACTIVE; // 0x91;
SomeRandomBloke 2:c19e63728e2e 157 i2c.write(addr, cmd, 2);
SomeRandomBloke 2:c19e63728e2e 158 }
SomeRandomBloke 2:c19e63728e2e 159
SomeRandomBloke 2:c19e63728e2e 160
SomeRandomBloke 0:26bfff579f01 161 void calXY() //magnetometer calibration: finding max and min of X, Y axis
SomeRandomBloke 0:26bfff579f01 162 {
SomeRandomBloke 0:26bfff579f01 163 int tempXmax, tempXmin, tempYmax, tempYmin, newX, newY;
SomeRandomBloke 1:d45fc466a46e 164 redLed = ON;
SomeRandomBloke 1:d45fc466a46e 165
SomeRandomBloke 2:c19e63728e2e 166 printf("Waiting for initial press\n");
SomeRandomBloke 2:c19e63728e2e 167 // Wait for slider to be pressed
SomeRandomBloke 2:c19e63728e2e 168 while( tsi.readDistance() == 0 ) {
SomeRandomBloke 2:c19e63728e2e 169 redLed = ON;
SomeRandomBloke 2:c19e63728e2e 170 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 171 redLed = OFF;
SomeRandomBloke 1:d45fc466a46e 172 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 173 }
SomeRandomBloke 2:c19e63728e2e 174
SomeRandomBloke 2:c19e63728e2e 175 printf("Waiting for release\n");
SomeRandomBloke 1:d45fc466a46e 176
SomeRandomBloke 1:d45fc466a46e 177 // Wait for release
SomeRandomBloke 1:d45fc466a46e 178 while( tsi.readDistance() != 0 ) {
SomeRandomBloke 1:d45fc466a46e 179 redLed = OFF;
SomeRandomBloke 1:d45fc466a46e 180 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 181 redLed = ON;
SomeRandomBloke 1:d45fc466a46e 182 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 183 }
SomeRandomBloke 1:d45fc466a46e 184 redLed = OFF;
SomeRandomBloke 2:c19e63728e2e 185 wait(0.5);
SomeRandomBloke 2:c19e63728e2e 186
SomeRandomBloke 2:c19e63728e2e 187 printf("Rotate\n");
SomeRandomBloke 1:d45fc466a46e 188
SomeRandomBloke 0:26bfff579f01 189 tempXmax = tempXmin = readVal(MAG_OUT_X_MSB);
SomeRandomBloke 0:26bfff579f01 190 tempYmax = tempYmin = readVal(MAG_OUT_Y_MSB);
SomeRandomBloke 2:c19e63728e2e 191
SomeRandomBloke 2:c19e63728e2e 192 while(tsi.readDistance() == 0) {
SomeRandomBloke 2:c19e63728e2e 193 greenLed = ON;
SomeRandomBloke 2:c19e63728e2e 194 wait(0.1);
SomeRandomBloke 2:c19e63728e2e 195 greenLed = OFF;
SomeRandomBloke 2:c19e63728e2e 196 wait(0.1);
SomeRandomBloke 0:26bfff579f01 197 newX = readVal(MAG_OUT_X_MSB);
SomeRandomBloke 0:26bfff579f01 198 newY = readVal(MAG_OUT_Y_MSB);
SomeRandomBloke 2:c19e63728e2e 199 if (newX > tempXmax) tempXmax = newX;
SomeRandomBloke 2:c19e63728e2e 200 if (newX < tempXmin) tempXmin = newX;
SomeRandomBloke 2:c19e63728e2e 201 if (newY > tempYmax) tempYmax = newY;
SomeRandomBloke 2:c19e63728e2e 202 if (newY < tempYmin) tempYmin = newY;
SomeRandomBloke 0:26bfff579f01 203 }
SomeRandomBloke 2:c19e63728e2e 204
SomeRandomBloke 0:26bfff579f01 205 settings.maxX = tempXmax;
SomeRandomBloke 0:26bfff579f01 206 settings.minX = tempXmin;
SomeRandomBloke 0:26bfff579f01 207 settings.maxY = tempYmax;
SomeRandomBloke 0:26bfff579f01 208 settings.minY = tempYmin;
SomeRandomBloke 0:26bfff579f01 209
SomeRandomBloke 2:c19e63728e2e 210 //store new X, Y values in EEPROM/Flash
SomeRandomBloke 0:26bfff579f01 211
SomeRandomBloke 2:c19e63728e2e 212 // Calculate average from min/max
SomeRandomBloke 0:26bfff579f01 213 avgX=(settings.maxX+settings.minX)/2;
SomeRandomBloke 0:26bfff579f01 214 avgY=(settings.maxY+settings.minY)/2;
SomeRandomBloke 0:26bfff579f01 215
SomeRandomBloke 1:d45fc466a46e 216 // Wait for release
SomeRandomBloke 1:d45fc466a46e 217 while( tsi.readDistance() != 0 ) {
SomeRandomBloke 1:d45fc466a46e 218 greenLed = OFF;
SomeRandomBloke 1:d45fc466a46e 219 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 220 greenLed = ON;
SomeRandomBloke 1:d45fc466a46e 221 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 222 }
SomeRandomBloke 1:d45fc466a46e 223 greenLed = OFF;
SomeRandomBloke 1:d45fc466a46e 224 wait(1.0);
SomeRandomBloke 1:d45fc466a46e 225
SomeRandomBloke 0:26bfff579f01 226
SomeRandomBloke 0:26bfff579f01 227 }
SomeRandomBloke 0:26bfff579f01 228
SomeRandomBloke 0:26bfff579f01 229 int main()
SomeRandomBloke 0:26bfff579f01 230 {
SomeRandomBloke 0:26bfff579f01 231 printf("MAG3110 Test\n");
SomeRandomBloke 0:26bfff579f01 232
SomeRandomBloke 1:d45fc466a46e 233 redLed = OFF;
SomeRandomBloke 1:d45fc466a46e 234 greenLed = OFF;
SomeRandomBloke 1:d45fc466a46e 235 blueLed = OFF;
SomeRandomBloke 0:26bfff579f01 236
SomeRandomBloke 2:c19e63728e2e 237 initMag();
SomeRandomBloke 0:26bfff579f01 238
SomeRandomBloke 0:26bfff579f01 239 // Get some values
SomeRandomBloke 0:26bfff579f01 240 printf("DR_STATUS %X\n", readReg( MAG_DR_STATUS ));
SomeRandomBloke 0:26bfff579f01 241 printf("WHO_AM_I %X\n", readReg( MAG_WHO_AM_I ));
SomeRandomBloke 0:26bfff579f01 242 printf("SYSMOD %X\n", readReg( MAG_SYSMOD ));
SomeRandomBloke 0:26bfff579f01 243 printf("DIE_TEMP %d\n", readReg( MAG_DIE_TEMP ));
SomeRandomBloke 0:26bfff579f01 244
SomeRandomBloke 0:26bfff579f01 245 printf("OFF_X %d\n", readVal( MAG_OFF_X_MSB ));
SomeRandomBloke 0:26bfff579f01 246 printf("OFF_Y %d\n", readVal( MAG_OFF_Y_MSB ));
SomeRandomBloke 0:26bfff579f01 247 printf("OFF_Z %d\n", readVal( MAG_OFF_Z_MSB ));
SomeRandomBloke 0:26bfff579f01 248
SomeRandomBloke 0:26bfff579f01 249 printf("CTRL_REG1 %X\n", readReg( MAG_CTRL_REG1 ));
SomeRandomBloke 0:26bfff579f01 250 printf("CTRL_REG2 %X\n", readReg( MAG_CTRL_REG2 ));
SomeRandomBloke 0:26bfff579f01 251
SomeRandomBloke 0:26bfff579f01 252 printf("calibrate\n");
SomeRandomBloke 0:26bfff579f01 253 calXY();
SomeRandomBloke 0:26bfff579f01 254 printf("....Finished\n");
SomeRandomBloke 0:26bfff579f01 255 printf("avgX = %d, avgY = %d\n", avgX, avgY);
SomeRandomBloke 0:26bfff579f01 256
SomeRandomBloke 1:d45fc466a46e 257 redLed = OFF;
SomeRandomBloke 1:d45fc466a46e 258 greenLed = OFF;
SomeRandomBloke 1:d45fc466a46e 259 blueLed = OFF;
SomeRandomBloke 0:26bfff579f01 260
SomeRandomBloke 0:26bfff579f01 261 while (1) {
SomeRandomBloke 0:26bfff579f01 262 wait(0.5);
SomeRandomBloke 0:26bfff579f01 263 int xVal = readVal(MAG_OUT_X_MSB);
SomeRandomBloke 0:26bfff579f01 264 int yVal = readVal(MAG_OUT_Y_MSB);
SomeRandomBloke 0:26bfff579f01 265 float heading = (atan2((double)(yVal-avgY),(double)(xVal-avgX)))*180/PI;
SomeRandomBloke 2:c19e63728e2e 266
SomeRandomBloke 2:c19e63728e2e 267 // Do something with heading - display direction and turn on blue LED if heading approx north
SomeRandomBloke 1:d45fc466a46e 268 if (abs(heading) <= 22.5) { printf("N\n"); blueLed = ON; } else blueLed = OFF;
SomeRandomBloke 0:26bfff579f01 269 if (abs(heading) >= 157.5) printf("S\n");
SomeRandomBloke 0:26bfff579f01 270 if (heading >= 67.5 && heading <= 112.5) printf("E \n");
SomeRandomBloke 0:26bfff579f01 271 if (heading <= -67.5 && heading >= -112.5) printf("W \n");
SomeRandomBloke 0:26bfff579f01 272 if (heading > 22.5 && heading < 67.5) printf("NE\n");
SomeRandomBloke 0:26bfff579f01 273 if (heading < -22.5 && heading > -67.5) printf("NW\n");
SomeRandomBloke 0:26bfff579f01 274 if (heading > 112.5 && heading < 157.5) printf("SE\n");
SomeRandomBloke 0:26bfff579f01 275 if (heading < -112.5 && heading > -157.5) printf("SW\n");
SomeRandomBloke 0:26bfff579f01 276
SomeRandomBloke 0:26bfff579f01 277 if (heading < 0) heading += 360.0;
SomeRandomBloke 0:26bfff579f01 278 printf("xVal - avgX = %d, yVal - avgY = %d ", xVal-avgX, yVal-avgY);
SomeRandomBloke 0:26bfff579f01 279 printf("X = %d, Y = %d, Heading %f\n", xVal, yVal, heading);
SomeRandomBloke 0:26bfff579f01 280
SomeRandomBloke 0:26bfff579f01 281 }
SomeRandomBloke 0:26bfff579f01 282 }