Andrew Lindsay / Mbed 2 deprecated Wi-Go-MagnetometerTest

Quick test of the Wi-Go Magnetometer

Dependencies:   TSI mbed MAG3110

main.cpp@1:d45fc466a46e, 2013-05-17 (annotated)

Committer:
SomeRandomBloke
Date:
Fri May 17 11:56:00 2013 +0000
Revision:
1:d45fc466a46e
Parent:
0:26bfff579f01
Child:
2:c19e63728e2e
updates

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SomeRandomBloke 0:26bfff579f01 1 #include "mbed.h"
SomeRandomBloke 1:d45fc466a46e 2 #include "TSISensor.h"
SomeRandomBloke 0:26bfff579f01 3 #include "math.h"
SomeRandomBloke 0:26bfff579f01 4
SomeRandomBloke 0:26bfff579f01 5 //PwmOut r (LED_RED);
SomeRandomBloke 0:26bfff579f01 6 //PwmOut g (LED_GREEN);
SomeRandomBloke 0:26bfff579f01 7 //PwmOut b (LED_BLUE);
SomeRandomBloke 0:26bfff579f01 8
SomeRandomBloke 0:26bfff579f01 9
SomeRandomBloke 0:26bfff579f01 10
SomeRandomBloke 0:26bfff579f01 11 #define MAG_ADDR 0x1D
SomeRandomBloke 0:26bfff579f01 12 // (0x0E << 1)
SomeRandomBloke 0:26bfff579f01 13 //0x1D
SomeRandomBloke 0:26bfff579f01 14
SomeRandomBloke 0:26bfff579f01 15 // define register values
SomeRandomBloke 0:26bfff579f01 16 #define MAG_DR_STATUS 0x00
SomeRandomBloke 0:26bfff579f01 17 #define MAG_OUT_X_MSB 0x01
SomeRandomBloke 0:26bfff579f01 18 #define MAG_OUT_X_LSB 0x02
SomeRandomBloke 0:26bfff579f01 19 #define MAG_OUT_Y_MSB 0x03
SomeRandomBloke 0:26bfff579f01 20 #define MAG_OUT_Y_LSB 0x04
SomeRandomBloke 0:26bfff579f01 21 #define MAG_OUT_Z_MSB 0x05
SomeRandomBloke 0:26bfff579f01 22 #define MAG_OUT_Z_LSB 0x06
SomeRandomBloke 0:26bfff579f01 23 #define MAG_WHO_AM_I 0x07
SomeRandomBloke 0:26bfff579f01 24 #define MAG_SYSMOD 0x08
SomeRandomBloke 0:26bfff579f01 25 #define MAG_OFF_X_MSB 0x09
SomeRandomBloke 0:26bfff579f01 26 #define MAG_OFF_X_LSB 0x0A
SomeRandomBloke 0:26bfff579f01 27 #define MAG_OFF_Y_MSB 0x0B
SomeRandomBloke 0:26bfff579f01 28 #define MAG_OFF_Y_LSB 0x0C
SomeRandomBloke 0:26bfff579f01 29 #define MAG_OFF_Z_MSB 0x0D
SomeRandomBloke 0:26bfff579f01 30 #define MAG_OFF_Z_LSB 0x0E
SomeRandomBloke 0:26bfff579f01 31 #define MAG_DIE_TEMP 0x0F
SomeRandomBloke 0:26bfff579f01 32 #define MAG_CTRL_REG1 0x10
SomeRandomBloke 0:26bfff579f01 33 #define MAG_CTRL_REG2 0x11
SomeRandomBloke 0:26bfff579f01 34
SomeRandomBloke 0:26bfff579f01 35 // what should WHO_AM_I return?
SomeRandomBloke 0:26bfff579f01 36 #define MAG_3110_WHO_AM_I_VALUE 0xC4
SomeRandomBloke 0:26bfff579f01 37
SomeRandomBloke 0:26bfff579f01 38
SomeRandomBloke 0:26bfff579f01 39 // Fields in registers
SomeRandomBloke 0:26bfff579f01 40 // CTRL_REG1: dr2,dr1,dr0 os1,os0 fr tm ac
SomeRandomBloke 0:26bfff579f01 41
SomeRandomBloke 0:26bfff579f01 42 // Sampling rate from 80Hz down to 0.625Hz
SomeRandomBloke 0:26bfff579f01 43 #define MAG_3110_SAMPLE80 0
SomeRandomBloke 0:26bfff579f01 44 #define MAG_3110_SAMPLE40 0x20
SomeRandomBloke 0:26bfff579f01 45 #define MAG_3110_SAMPLE20 0x40
SomeRandomBloke 0:26bfff579f01 46 #define MAG_3110_SAMPLE10 0x60
SomeRandomBloke 0:26bfff579f01 47 #define MAG_3110_SAMPLE5 0x80
SomeRandomBloke 0:26bfff579f01 48 #define MAG_3110_SAMPLE2_5 0xA0
SomeRandomBloke 0:26bfff579f01 49 #define MAG_3110_SAMPLE1_25 0xC0
SomeRandomBloke 0:26bfff579f01 50 #define MAG_3110_SAMPLE0_625 0xE0
SomeRandomBloke 0:26bfff579f01 51
SomeRandomBloke 0:26bfff579f01 52 // How many samples to average (lowers data rate)
SomeRandomBloke 0:26bfff579f01 53 #define MAG_3110_OVERSAMPLE1 0
SomeRandomBloke 0:26bfff579f01 54 #define MAG_3110_OVERSAMPLE2 0x08
SomeRandomBloke 0:26bfff579f01 55 #define MAG_3110_OVERSAMPLE3 0x10
SomeRandomBloke 0:26bfff579f01 56 #define MAG_3110_OVERSAMPLE4 0x18
SomeRandomBloke 0:26bfff579f01 57
SomeRandomBloke 0:26bfff579f01 58 // read only 1 byte per axis
SomeRandomBloke 0:26bfff579f01 59 #define MAG_3110_FASTREAD 0x04
SomeRandomBloke 0:26bfff579f01 60 // do one measurement (even if in standby mode)
SomeRandomBloke 0:26bfff579f01 61 #define MAG_3110_TRIGGER 0x02
SomeRandomBloke 0:26bfff579f01 62 // put in active mode
SomeRandomBloke 0:26bfff579f01 63 #define MAG_3110_ACTIVE 0x01
SomeRandomBloke 0:26bfff579f01 64
SomeRandomBloke 0:26bfff579f01 65 // CTRL_REG2: AUTO_MRST_EN _ RAW MAG_RST _ _ _ _ _
SomeRandomBloke 0:26bfff579f01 66 // reset sensor after each reading
SomeRandomBloke 0:26bfff579f01 67 #define MAG_3110_AUTO_MRST_EN 0x80
SomeRandomBloke 0:26bfff579f01 68 // don't subtract user offsets
SomeRandomBloke 0:26bfff579f01 69 #define MAG_3110_RAW 0x20
SomeRandomBloke 0:26bfff579f01 70 // reset magnetic sensor after too-large field
SomeRandomBloke 0:26bfff579f01 71 #define MAG_3110_MAG_RST 0x10
SomeRandomBloke 0:26bfff579f01 72
SomeRandomBloke 0:26bfff579f01 73 // DR_STATUS Register ZYXOW ZOW YOW XOW ZYXDR ZDR YDR XDR
SomeRandomBloke 0:26bfff579f01 74 #define MAG_3110_ZYXDR 0x08
SomeRandomBloke 0:26bfff579f01 75
SomeRandomBloke 0:26bfff579f01 76
SomeRandomBloke 0:26bfff579f01 77 //0x0E //7-bit address for the MAG3110, doesn't change
SomeRandomBloke 0:26bfff579f01 78
SomeRandomBloke 0:26bfff579f01 79 DigitalOut myled(PTB10);
SomeRandomBloke 0:26bfff579f01 80 DigitalOut redLed(LED_RED);
SomeRandomBloke 0:26bfff579f01 81 DigitalOut greenLed(LED_GREEN);
SomeRandomBloke 0:26bfff579f01 82 DigitalOut blueLed(LED_BLUE);
SomeRandomBloke 0:26bfff579f01 83
SomeRandomBloke 1:d45fc466a46e 84 TSISensor tsi;
SomeRandomBloke 1:d45fc466a46e 85
SomeRandomBloke 0:26bfff579f01 86 I2C i2c(PTE0, PTE1);
SomeRandomBloke 0:26bfff579f01 87
SomeRandomBloke 0:26bfff579f01 88 int avgX, avgY, newX, tempXmin, tempXmax, newY, tempYmin, tempYmax;
SomeRandomBloke 0:26bfff579f01 89 struct settings_t {
SomeRandomBloke 0:26bfff579f01 90 long maxX, minX, maxY, minY;
SomeRandomBloke 0:26bfff579f01 91 }
SomeRandomBloke 0:26bfff579f01 92 settings;
SomeRandomBloke 0:26bfff579f01 93
SomeRandomBloke 0:26bfff579f01 94 const int addr = MAG_ADDR;
SomeRandomBloke 0:26bfff579f01 95
SomeRandomBloke 0:26bfff579f01 96 // Read a single byte form 8 bit register, return as int
SomeRandomBloke 0:26bfff579f01 97 int readReg(char regAddr)
SomeRandomBloke 0:26bfff579f01 98 {
SomeRandomBloke 0:26bfff579f01 99 char cmd[1];
SomeRandomBloke 0:26bfff579f01 100
SomeRandomBloke 0:26bfff579f01 101 cmd[0] = regAddr;
SomeRandomBloke 0:26bfff579f01 102 i2c.write(addr, cmd, 1);
SomeRandomBloke 0:26bfff579f01 103
SomeRandomBloke 0:26bfff579f01 104 cmd[0] = 0x00;
SomeRandomBloke 0:26bfff579f01 105 i2c.read(addr, cmd, 1);
SomeRandomBloke 0:26bfff579f01 106 return (int)( cmd[0]);
SomeRandomBloke 0:26bfff579f01 107
SomeRandomBloke 0:26bfff579f01 108 }
SomeRandomBloke 0:26bfff579f01 109
SomeRandomBloke 0:26bfff579f01 110
SomeRandomBloke 0:26bfff579f01 111 // read a register per, pass first reg value, reading 2 bytes increments register
SomeRandomBloke 0:26bfff579f01 112 // Reads MSB first then LSB
SomeRandomBloke 0:26bfff579f01 113 int readVal(char regAddr)
SomeRandomBloke 0:26bfff579f01 114 {
SomeRandomBloke 0:26bfff579f01 115 char cmd[2];
SomeRandomBloke 0:26bfff579f01 116
SomeRandomBloke 0:26bfff579f01 117 cmd[0] = regAddr;
SomeRandomBloke 0:26bfff579f01 118 i2c.write(addr, cmd, 1);
SomeRandomBloke 0:26bfff579f01 119
SomeRandomBloke 0:26bfff579f01 120 cmd[0] = 0x00;
SomeRandomBloke 0:26bfff579f01 121 cmd[1] = 0x00;
SomeRandomBloke 0:26bfff579f01 122 i2c.read(addr, cmd, 2);
SomeRandomBloke 0:26bfff579f01 123 return (int)( (cmd[1]|(cmd[0] << 8))); //concatenate the MSB and LSB
SomeRandomBloke 0:26bfff579f01 124 }
SomeRandomBloke 0:26bfff579f01 125
SomeRandomBloke 0:26bfff579f01 126 #define PI 3.14159265359
SomeRandomBloke 1:d45fc466a46e 127 #define ON 0
SomeRandomBloke 1:d45fc466a46e 128 #define OFF 1
SomeRandomBloke 0:26bfff579f01 129
SomeRandomBloke 0:26bfff579f01 130 void calXY() //magnetometer calibration: finding max and min of X, Y axis
SomeRandomBloke 0:26bfff579f01 131 {
SomeRandomBloke 0:26bfff579f01 132 int tempXmax, tempXmin, tempYmax, tempYmin, newX, newY;
SomeRandomBloke 1:d45fc466a46e 133 redLed = ON;
SomeRandomBloke 0:26bfff579f01 134 // lcd.setCursor(0,1);
SomeRandomBloke 0:26bfff579f01 135 // lcd.print(“Rotate the car “);
SomeRandomBloke 0:26bfff579f01 136 // delay(1000);
SomeRandomBloke 0:26bfff579f01 137 // lcd.setCursor(0,1);
SomeRandomBloke 0:26bfff579f01 138 // lcd.print(“and press button”);
SomeRandomBloke 0:26bfff579f01 139 // delay(1000);
SomeRandomBloke 0:26bfff579f01 140 // lcd.setCursor(0,1);
SomeRandomBloke 0:26bfff579f01 141 // lcd.print(“Now, begin! “);
SomeRandomBloke 0:26bfff579f01 142 // delay(500);
SomeRandomBloke 1:d45fc466a46e 143
SomeRandomBloke 1:d45fc466a46e 144 // Wait for slider to be pressed at one end > 30mm
SomeRandomBloke 1:d45fc466a46e 145 while( tsi.readDistance() < 30 && tsi.readDistance() != 0 ) {
SomeRandomBloke 1:d45fc466a46e 146 redLed = OFF;
SomeRandomBloke 1:d45fc466a46e 147 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 148 redLed = ON;
SomeRandomBloke 1:d45fc466a46e 149 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 150 }
SomeRandomBloke 1:d45fc466a46e 151
SomeRandomBloke 1:d45fc466a46e 152 // Wait for release
SomeRandomBloke 1:d45fc466a46e 153 while( tsi.readDistance() != 0 ) {
SomeRandomBloke 1:d45fc466a46e 154 redLed = OFF;
SomeRandomBloke 1:d45fc466a46e 155 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 156 redLed = ON;
SomeRandomBloke 1:d45fc466a46e 157 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 158 }
SomeRandomBloke 1:d45fc466a46e 159 redLed = OFF;
SomeRandomBloke 1:d45fc466a46e 160 wait(1.0);
SomeRandomBloke 1:d45fc466a46e 161
SomeRandomBloke 0:26bfff579f01 162 tempXmax = tempXmin = readVal(MAG_OUT_X_MSB);
SomeRandomBloke 0:26bfff579f01 163 tempYmax = tempYmin = readVal(MAG_OUT_Y_MSB);
SomeRandomBloke 0:26bfff579f01 164 int changeCount = 10000;
SomeRandomBloke 0:26bfff579f01 165 bool change = false;
SomeRandomBloke 0:26bfff579f01 166 while(changeCount) { // digitalRead(10) == LOW) {
SomeRandomBloke 0:26bfff579f01 167 // for(int x=0; x<100000; x++); {
SomeRandomBloke 0:26bfff579f01 168 //newX = readx();
SomeRandomBloke 0:26bfff579f01 169 //newY = ready();
SomeRandomBloke 0:26bfff579f01 170 newX = readVal(MAG_OUT_X_MSB);
SomeRandomBloke 0:26bfff579f01 171 newY = readVal(MAG_OUT_Y_MSB);
SomeRandomBloke 0:26bfff579f01 172 if (newX > tempXmax) {
SomeRandomBloke 0:26bfff579f01 173 tempXmax = newX;
SomeRandomBloke 0:26bfff579f01 174 change = true;
SomeRandomBloke 0:26bfff579f01 175 }
SomeRandomBloke 0:26bfff579f01 176 if (newX < tempXmin) {
SomeRandomBloke 0:26bfff579f01 177 tempXmin = newX;
SomeRandomBloke 0:26bfff579f01 178 change = true;
SomeRandomBloke 0:26bfff579f01 179 }
SomeRandomBloke 0:26bfff579f01 180 if (newY > tempYmax) {
SomeRandomBloke 0:26bfff579f01 181 tempYmax = newY;
SomeRandomBloke 0:26bfff579f01 182 change = true;
SomeRandomBloke 0:26bfff579f01 183 }
SomeRandomBloke 0:26bfff579f01 184 if (newY < tempYmin) {
SomeRandomBloke 0:26bfff579f01 185 tempYmin = newY;
SomeRandomBloke 0:26bfff579f01 186 change = true;
SomeRandomBloke 0:26bfff579f01 187 }
SomeRandomBloke 0:26bfff579f01 188 if( change )
SomeRandomBloke 0:26bfff579f01 189 change = false;
SomeRandomBloke 0:26bfff579f01 190 else
SomeRandomBloke 0:26bfff579f01 191 changeCount--;
SomeRandomBloke 0:26bfff579f01 192 // printf("X max %d min %d, Y max %d min %d\n",tempXmax, tempXmin, tempYmax, tempYmin);
SomeRandomBloke 0:26bfff579f01 193 }
SomeRandomBloke 0:26bfff579f01 194 settings.maxX = tempXmax;
SomeRandomBloke 0:26bfff579f01 195 settings.minX = tempXmin;
SomeRandomBloke 0:26bfff579f01 196 settings.maxY = tempYmax;
SomeRandomBloke 0:26bfff579f01 197 settings.minY = tempYmin;
SomeRandomBloke 0:26bfff579f01 198 //X max 65173 min 64850, Y max 490 min 141
SomeRandomBloke 0:26bfff579f01 199
SomeRandomBloke 0:26bfff579f01 200 //store new X, Y values in the EEPROM
SomeRandomBloke 0:26bfff579f01 201 // eeprom_write_block((const void*)&settings, (void*)0, sizeof(settings));
SomeRandomBloke 0:26bfff579f01 202
SomeRandomBloke 0:26bfff579f01 203 avgX=(settings.maxX+settings.minX)/2;
SomeRandomBloke 0:26bfff579f01 204 avgY=(settings.maxY+settings.minY)/2;
SomeRandomBloke 0:26bfff579f01 205
SomeRandomBloke 1:d45fc466a46e 206 redLed = OFF;
SomeRandomBloke 1:d45fc466a46e 207 // Wait for slider to be pressed at one end > 30mm
SomeRandomBloke 1:d45fc466a46e 208 while( tsi.readDistance() < 30 && tsi.readDistance() != 0 ) {
SomeRandomBloke 1:d45fc466a46e 209 greenLed = OFF;
SomeRandomBloke 1:d45fc466a46e 210 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 211 greenLed = ON;
SomeRandomBloke 1:d45fc466a46e 212 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 213 }
SomeRandomBloke 1:d45fc466a46e 214
SomeRandomBloke 1:d45fc466a46e 215 // Wait for release
SomeRandomBloke 1:d45fc466a46e 216 while( tsi.readDistance() != 0 ) {
SomeRandomBloke 1:d45fc466a46e 217 greenLed = OFF;
SomeRandomBloke 1:d45fc466a46e 218 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 219 greenLed = ON;
SomeRandomBloke 1:d45fc466a46e 220 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 221 }
SomeRandomBloke 1:d45fc466a46e 222 greenLed = OFF;
SomeRandomBloke 1:d45fc466a46e 223 wait(1.0);
SomeRandomBloke 1:d45fc466a46e 224
SomeRandomBloke 0:26bfff579f01 225 // lcd.setCursor(0,1);
SomeRandomBloke 0:26bfff579f01 226 // lcd.print(“Calibration done”);
SomeRandomBloke 0:26bfff579f01 227 //delay(3000);
SomeRandomBloke 0:26bfff579f01 228 // lcd.setCursor(0,1);
SomeRandomBloke 0:26bfff579f01 229 // lcd.print(” “);
SomeRandomBloke 0:26bfff579f01 230
SomeRandomBloke 0:26bfff579f01 231 }
SomeRandomBloke 0:26bfff579f01 232
SomeRandomBloke 0:26bfff579f01 233 int main()
SomeRandomBloke 0:26bfff579f01 234 {
SomeRandomBloke 0:26bfff579f01 235 char cmd[2];
SomeRandomBloke 0:26bfff579f01 236 // r.period(0.001);
SomeRandomBloke 0:26bfff579f01 237 // g.period(0.001);
SomeRandomBloke 0:26bfff579f01 238 // b.period(0.001);
SomeRandomBloke 0:26bfff579f01 239
SomeRandomBloke 0:26bfff579f01 240 printf("MAG3110 Test\n");
SomeRandomBloke 0:26bfff579f01 241
SomeRandomBloke 1:d45fc466a46e 242 redLed = OFF;
SomeRandomBloke 1:d45fc466a46e 243 greenLed = OFF;
SomeRandomBloke 1:d45fc466a46e 244 blueLed = OFF;
SomeRandomBloke 0:26bfff579f01 245
SomeRandomBloke 0:26bfff579f01 246 cmd[0] = MAG_CTRL_REG2;
SomeRandomBloke 0:26bfff579f01 247 cmd[1] = 0x80;
SomeRandomBloke 0:26bfff579f01 248 i2c.write(addr, cmd, 2);
SomeRandomBloke 0:26bfff579f01 249
SomeRandomBloke 0:26bfff579f01 250 // wait(0.1);
SomeRandomBloke 0:26bfff579f01 251 cmd[0] = MAG_CTRL_REG1;
SomeRandomBloke 0:26bfff579f01 252 cmd[1] = MAG_3110_SAMPLE80+MAG_3110_OVERSAMPLE2+MAG_3110_ACTIVE; // 0x91;
SomeRandomBloke 0:26bfff579f01 253 i2c.write(addr, cmd, 2);
SomeRandomBloke 0:26bfff579f01 254
SomeRandomBloke 0:26bfff579f01 255 // Get some values
SomeRandomBloke 0:26bfff579f01 256 printf("DR_STATUS %X\n", readReg( MAG_DR_STATUS ));
SomeRandomBloke 0:26bfff579f01 257 printf("WHO_AM_I %X\n", readReg( MAG_WHO_AM_I ));
SomeRandomBloke 0:26bfff579f01 258 printf("SYSMOD %X\n", readReg( MAG_SYSMOD ));
SomeRandomBloke 0:26bfff579f01 259 printf("DIE_TEMP %d\n", readReg( MAG_DIE_TEMP ));
SomeRandomBloke 0:26bfff579f01 260
SomeRandomBloke 0:26bfff579f01 261 printf("OFF_X %d\n", readVal( MAG_OFF_X_MSB ));
SomeRandomBloke 0:26bfff579f01 262 printf("OFF_Y %d\n", readVal( MAG_OFF_Y_MSB ));
SomeRandomBloke 0:26bfff579f01 263 printf("OFF_Z %d\n", readVal( MAG_OFF_Z_MSB ));
SomeRandomBloke 0:26bfff579f01 264
SomeRandomBloke 0:26bfff579f01 265 printf("CTRL_REG1 %X\n", readReg( MAG_CTRL_REG1 ));
SomeRandomBloke 0:26bfff579f01 266 printf("CTRL_REG2 %X\n", readReg( MAG_CTRL_REG2 ));
SomeRandomBloke 0:26bfff579f01 267
SomeRandomBloke 0:26bfff579f01 268 // r = 1;
SomeRandomBloke 0:26bfff579f01 269 // g = 0;
SomeRandomBloke 0:26bfff579f01 270 // b = 0;
SomeRandomBloke 0:26bfff579f01 271 settings.maxX = 65173;
SomeRandomBloke 0:26bfff579f01 272 settings.minX = 64850;
SomeRandomBloke 0:26bfff579f01 273 settings.maxY = 490;
SomeRandomBloke 0:26bfff579f01 274 settings.minY = 141;
SomeRandomBloke 0:26bfff579f01 275 //X max 65173 min 64850, Y max 490 min 141
SomeRandomBloke 0:26bfff579f01 276 avgX=(settings.maxX+settings.minX)/2;
SomeRandomBloke 0:26bfff579f01 277 avgY=(settings.maxY+settings.minY)/2;
SomeRandomBloke 0:26bfff579f01 278 printf("avgX = %d, avgY = %d\n", avgX, avgY);
SomeRandomBloke 0:26bfff579f01 279
SomeRandomBloke 0:26bfff579f01 280 // avgX = 0;
SomeRandomBloke 0:26bfff579f01 281 // avgY = 0;
SomeRandomBloke 0:26bfff579f01 282
SomeRandomBloke 0:26bfff579f01 283 printf("calibrate\n");
SomeRandomBloke 0:26bfff579f01 284 calXY();
SomeRandomBloke 0:26bfff579f01 285 printf("....Finished\n");
SomeRandomBloke 0:26bfff579f01 286 printf("avgX = %d, avgY = %d\n", avgX, avgY);
SomeRandomBloke 0:26bfff579f01 287
SomeRandomBloke 0:26bfff579f01 288 // greenLed = 1;
SomeRandomBloke 1:d45fc466a46e 289 redLed = OFF;
SomeRandomBloke 1:d45fc466a46e 290 greenLed = OFF;
SomeRandomBloke 1:d45fc466a46e 291 blueLed = OFF;
SomeRandomBloke 0:26bfff579f01 292
SomeRandomBloke 0:26bfff579f01 293 while (1) {
SomeRandomBloke 0:26bfff579f01 294 wait(0.5);
SomeRandomBloke 0:26bfff579f01 295 int xVal = readVal(MAG_OUT_X_MSB);
SomeRandomBloke 0:26bfff579f01 296 int yVal = readVal(MAG_OUT_Y_MSB);
SomeRandomBloke 0:26bfff579f01 297 float heading = (atan2((double)(yVal-avgY),(double)(xVal-avgX)))*180/PI;
SomeRandomBloke 0:26bfff579f01 298 // float heading = (atan2((double)(xVal-avgX),(double)(yVal-avgY)))*180/PI;
SomeRandomBloke 0:26bfff579f01 299
SomeRandomBloke 1:d45fc466a46e 300 if (abs(heading) <= 22.5) { printf("N\n"); blueLed = ON; } else blueLed = OFF;
SomeRandomBloke 0:26bfff579f01 301 if (abs(heading) >= 157.5) printf("S\n");
SomeRandomBloke 0:26bfff579f01 302 if (heading >= 67.5 && heading <= 112.5) printf("E \n");
SomeRandomBloke 0:26bfff579f01 303 if (heading <= -67.5 && heading >= -112.5) printf("W \n");
SomeRandomBloke 0:26bfff579f01 304 if (heading > 22.5 && heading < 67.5) printf("NE\n");
SomeRandomBloke 0:26bfff579f01 305 if (heading < -22.5 && heading > -67.5) printf("NW\n");
SomeRandomBloke 0:26bfff579f01 306 if (heading > 112.5 && heading < 157.5) printf("SE\n");
SomeRandomBloke 0:26bfff579f01 307 if (heading < -112.5 && heading > -157.5) printf("SW\n");
SomeRandomBloke 0:26bfff579f01 308
SomeRandomBloke 0:26bfff579f01 309 if (heading < 0) heading += 360.0;
SomeRandomBloke 0:26bfff579f01 310 printf("xVal - avgX = %d, yVal - avgY = %d ", xVal-avgX, yVal-avgY);
SomeRandomBloke 0:26bfff579f01 311 printf("X = %d, Y = %d, Heading %f\n", xVal, yVal, heading);
SomeRandomBloke 0:26bfff579f01 312
SomeRandomBloke 0:26bfff579f01 313 // printf("2 byte: X = %d ", readVal( 0x01 ));
SomeRandomBloke 0:26bfff579f01 314 // printf(" Y = %d ", readVal( 0x03 ));
SomeRandomBloke 0:26bfff579f01 315 // printf(" Z = %d\n", readVal( 0x05 ));
SomeRandomBloke 0:26bfff579f01 316
SomeRandomBloke 0:26bfff579f01 317 // printf("1 byte: X = %d ", readx());
SomeRandomBloke 0:26bfff579f01 318 // printf(" Y = %d ", ready());
SomeRandomBloke 0:26bfff579f01 319 // printf(" Z = %d\n", readz());
SomeRandomBloke 0:26bfff579f01 320 }
SomeRandomBloke 0:26bfff579f01 321 }