
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?
User | Revision | Line number | New 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 | } |