Quick test of the Wi-Go Magnetometer
Dependencies: TSI mbed MAG3110
Diff: main.cpp
- Revision:
- 0:26bfff579f01
- Child:
- 1:d45fc466a46e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu May 16 22:11:33 2013 +0000 @@ -0,0 +1,282 @@ +#include "mbed.h" +#include "math.h" + +//PwmOut r (LED_RED); +//PwmOut g (LED_GREEN); +//PwmOut b (LED_BLUE); + + + +#define MAG_ADDR 0x1D +// (0x0E << 1) +//0x1D + +// define register values +#define MAG_DR_STATUS 0x00 +#define MAG_OUT_X_MSB 0x01 +#define MAG_OUT_X_LSB 0x02 +#define MAG_OUT_Y_MSB 0x03 +#define MAG_OUT_Y_LSB 0x04 +#define MAG_OUT_Z_MSB 0x05 +#define MAG_OUT_Z_LSB 0x06 +#define MAG_WHO_AM_I 0x07 +#define MAG_SYSMOD 0x08 +#define MAG_OFF_X_MSB 0x09 +#define MAG_OFF_X_LSB 0x0A +#define MAG_OFF_Y_MSB 0x0B +#define MAG_OFF_Y_LSB 0x0C +#define MAG_OFF_Z_MSB 0x0D +#define MAG_OFF_Z_LSB 0x0E +#define MAG_DIE_TEMP 0x0F +#define MAG_CTRL_REG1 0x10 +#define MAG_CTRL_REG2 0x11 + +// what should WHO_AM_I return? +#define MAG_3110_WHO_AM_I_VALUE 0xC4 + + +// Fields in registers +// CTRL_REG1: dr2,dr1,dr0 os1,os0 fr tm ac + +// Sampling rate from 80Hz down to 0.625Hz +#define MAG_3110_SAMPLE80 0 +#define MAG_3110_SAMPLE40 0x20 +#define MAG_3110_SAMPLE20 0x40 +#define MAG_3110_SAMPLE10 0x60 +#define MAG_3110_SAMPLE5 0x80 +#define MAG_3110_SAMPLE2_5 0xA0 +#define MAG_3110_SAMPLE1_25 0xC0 +#define MAG_3110_SAMPLE0_625 0xE0 + +// How many samples to average (lowers data rate) +#define MAG_3110_OVERSAMPLE1 0 +#define MAG_3110_OVERSAMPLE2 0x08 +#define MAG_3110_OVERSAMPLE3 0x10 +#define MAG_3110_OVERSAMPLE4 0x18 + +// read only 1 byte per axis +#define MAG_3110_FASTREAD 0x04 +// do one measurement (even if in standby mode) +#define MAG_3110_TRIGGER 0x02 +// put in active mode +#define MAG_3110_ACTIVE 0x01 + +// CTRL_REG2: AUTO_MRST_EN _ RAW MAG_RST _ _ _ _ _ +// reset sensor after each reading +#define MAG_3110_AUTO_MRST_EN 0x80 +// don't subtract user offsets +#define MAG_3110_RAW 0x20 +// reset magnetic sensor after too-large field +#define MAG_3110_MAG_RST 0x10 + +// DR_STATUS Register ZYXOW ZOW YOW XOW ZYXDR ZDR YDR XDR +#define MAG_3110_ZYXDR 0x08 + + +//0x0E //7-bit address for the MAG3110, doesn't change + +DigitalOut myled(PTB10); +DigitalOut redLed(LED_RED); +DigitalOut greenLed(LED_GREEN); +DigitalOut blueLed(LED_BLUE); + +I2C i2c(PTE0, PTE1); + +int avgX, avgY, newX, tempXmin, tempXmax, newY, tempYmin, tempYmax; +struct settings_t { + long maxX, minX, maxY, minY; +} +settings; + +const int addr = MAG_ADDR; + +// Read a single byte form 8 bit register, return as int +int readReg(char regAddr) +{ + char cmd[1]; + + cmd[0] = regAddr; + i2c.write(addr, cmd, 1); + + cmd[0] = 0x00; + i2c.read(addr, cmd, 1); + return (int)( cmd[0]); + +} + + +// read a register per, pass first reg value, reading 2 bytes increments register +// Reads MSB first then LSB +int readVal(char regAddr) +{ + char cmd[2]; + + cmd[0] = regAddr; + i2c.write(addr, cmd, 1); + + cmd[0] = 0x00; + cmd[1] = 0x00; + i2c.read(addr, cmd, 2); + return (int)( (cmd[1]|(cmd[0] << 8))); //concatenate the MSB and LSB +} + +#define PI 3.14159265359 + + +void calXY() //magnetometer calibration: finding max and min of X, Y axis +{ + int tempXmax, tempXmin, tempYmax, tempYmin, newX, newY; + redLed = 0; +// lcd.setCursor(0,1); +// lcd.print(“Rotate the car “); +// delay(1000); +// lcd.setCursor(0,1); +// lcd.print(“and press button”); +// delay(1000); +// lcd.setCursor(0,1); +// lcd.print(“Now, begin! “); +// delay(500); + tempXmax = tempXmin = readVal(MAG_OUT_X_MSB); + tempYmax = tempYmin = readVal(MAG_OUT_Y_MSB); + int changeCount = 10000; + bool change = false; + while(changeCount) { // digitalRead(10) == LOW) { +// for(int x=0; x<100000; x++); { + //newX = readx(); + //newY = ready(); + newX = readVal(MAG_OUT_X_MSB); + newY = readVal(MAG_OUT_Y_MSB); + if (newX > tempXmax) { + tempXmax = newX; + change = true; + } + if (newX < tempXmin) { + tempXmin = newX; + change = true; + } + if (newY > tempYmax) { + tempYmax = newY; + change = true; + } + if (newY < tempYmin) { + tempYmin = newY; + change = true; + } + if( change ) + change = false; + else + changeCount--; +// printf("X max %d min %d, Y max %d min %d\n",tempXmax, tempXmin, tempYmax, tempYmin); + } + settings.maxX = tempXmax; + settings.minX = tempXmin; + settings.maxY = tempYmax; + settings.minY = tempYmin; + //X max 65173 min 64850, Y max 490 min 141 + +//store new X, Y values in the EEPROM +// eeprom_write_block((const void*)&settings, (void*)0, sizeof(settings)); + + avgX=(settings.maxX+settings.minX)/2; + avgY=(settings.maxY+settings.minY)/2; + + redLed = 1; +// lcd.setCursor(0,1); +// lcd.print(“Calibration done”); + //delay(3000); +// lcd.setCursor(0,1); +// lcd.print(” “); + +} + +int main() +{ + char cmd[2]; +// r.period(0.001); +// g.period(0.001); +// b.period(0.001); + + printf("MAG3110 Test\n"); + + redLed = 1; + greenLed = 1; + blueLed = 1; + + cmd[0] = MAG_CTRL_REG2; + cmd[1] = 0x80; + i2c.write(addr, cmd, 2); + +// wait(0.1); + cmd[0] = MAG_CTRL_REG1; + cmd[1] = MAG_3110_SAMPLE80+MAG_3110_OVERSAMPLE2+MAG_3110_ACTIVE; // 0x91; + i2c.write(addr, cmd, 2); + + // Get some values + printf("DR_STATUS %X\n", readReg( MAG_DR_STATUS )); + printf("WHO_AM_I %X\n", readReg( MAG_WHO_AM_I )); + printf("SYSMOD %X\n", readReg( MAG_SYSMOD )); + printf("DIE_TEMP %d\n", readReg( MAG_DIE_TEMP )); + + printf("OFF_X %d\n", readVal( MAG_OFF_X_MSB )); + printf("OFF_Y %d\n", readVal( MAG_OFF_Y_MSB )); + printf("OFF_Z %d\n", readVal( MAG_OFF_Z_MSB )); + + printf("CTRL_REG1 %X\n", readReg( MAG_CTRL_REG1 )); + printf("CTRL_REG2 %X\n", readReg( MAG_CTRL_REG2 )); + +// r = 1; +// g = 0; +// b = 0; + settings.maxX = 65173; + settings.minX = 64850; + settings.maxY = 490; + settings.minY = 141; + //X max 65173 min 64850, Y max 490 min 141 + avgX=(settings.maxX+settings.minX)/2; + avgY=(settings.maxY+settings.minY)/2; + printf("avgX = %d, avgY = %d\n", avgX, avgY); + +// avgX = 0; +// avgY = 0; + + printf("calibrate\n"); + calXY(); + printf("....Finished\n"); + printf("avgX = %d, avgY = %d\n", avgX, avgY); + + // greenLed = 1; + redLed = 1; + greenLed = 1; + blueLed = 1; + + while (1) { + wait(0.5); + int xVal = readVal(MAG_OUT_X_MSB); + int yVal = readVal(MAG_OUT_Y_MSB); + float heading = (atan2((double)(yVal-avgY),(double)(xVal-avgX)))*180/PI; +// float heading = (atan2((double)(xVal-avgX),(double)(yVal-avgY)))*180/PI; + + if (abs(heading) <= 22.5) { printf("N\n"); greenLed = 0; } else greenLed = 1; + if (abs(heading) >= 157.5) printf("S\n"); + if (heading >= 67.5 && heading <= 112.5) printf("E \n"); + if (heading <= -67.5 && heading >= -112.5) printf("W \n"); + if (heading > 22.5 && heading < 67.5) printf("NE\n"); + if (heading < -22.5 && heading > -67.5) printf("NW\n"); + if (heading > 112.5 && heading < 157.5) printf("SE\n"); + if (heading < -112.5 && heading > -157.5) printf("SW\n"); + + if (heading < 0) heading += 360.0; + printf("xVal - avgX = %d, yVal - avgY = %d ", xVal-avgX, yVal-avgY); + printf("X = %d, Y = %d, Heading %f\n", xVal, yVal, heading); + + + +// printf("2 byte: X = %d ", readVal( 0x01 )); +// printf(" Y = %d ", readVal( 0x03 )); +// printf(" Z = %d\n", readVal( 0x05 )); + +// printf("1 byte: X = %d ", readx()); +// printf(" Y = %d ", ready()); +// printf(" Z = %d\n", readz()); + } +}