Quick test of the Wi-Go Magnetometer
Dependencies: TSI mbed MAG3110
Diff: main.cpp
- Revision:
- 2:c19e63728e2e
- Parent:
- 1:d45fc466a46e
- Child:
- 3:945e32be0448
diff -r d45fc466a46e -r c19e63728e2e main.cpp --- a/main.cpp Fri May 17 11:56:00 2013 +0000 +++ b/main.cpp Sat May 18 10:00:03 2013 +0000 @@ -1,18 +1,35 @@ +/** + * Simple MAG3110 test/demo program, based on various bits of code found. + * Updated for Mbed FRDM-KL25Z and Avnet Wi-Go module. + * + * Operation: + * 1. On startup Red LED flashes indicating calibration mode entered + * 2. Slide finger along capacitive sensor and release + * 3. Green LED flashes indicating calibration mode. + * 4. Rotate board once in horizontal plane + * 5. Tap and release capacitive sensor. Board now calibrated with min/max values + * 6. LEDs now off. Rotate board. When Blue LED lights the bottom of the board is + * pointing to approximately North (+/- 22.5') + * + * 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. + * + * Also displays register values when starting. + * + * Ideally the calibration settings would be stored in flash/eeprom and retrieves each time. + * + * By Andrew D. Lindsay, @AndrewDLindsay + * + * + * + */ + #include "mbed.h" #include "TSISensor.h" #include "math.h" -//PwmOut r (LED_RED); -//PwmOut g (LED_GREEN); -//PwmOut b (LED_BLUE); - - +#define MAG_ADDR 0x1D -#define MAG_ADDR 0x1D -// (0x0E << 1) -//0x1D - -// define register values +// define registers #define MAG_DR_STATUS 0x00 #define MAG_OUT_X_MSB 0x01 #define MAG_OUT_X_LSB 0x02 @@ -74,18 +91,23 @@ #define MAG_3110_ZYXDR 0x08 -//0x0E //7-bit address for the MAG3110, doesn't change +#define PI 3.14159265359 +#define ON 0 +#define OFF 1 -DigitalOut myled(PTB10); +// Some LEDs for showing status DigitalOut redLed(LED_RED); DigitalOut greenLed(LED_GREEN); DigitalOut blueLed(LED_BLUE); +// Slide sensor acts as a button TSISensor tsi; - + +// I2C used to communicate with sensor I2C i2c(PTE0, PTE1); int avgX, avgY, newX, tempXmin, tempXmax, newY, tempYmin, tempYmax; +// Ideally these would be saved in eeprom/flash struct settings_t { long maxX, minX, maxY, minY; } @@ -104,7 +126,6 @@ cmd[0] = 0x00; i2c.read(addr, cmd, 1); return (int)( cmd[0]); - } @@ -123,31 +144,35 @@ return (int)( (cmd[1]|(cmd[0] << 8))); //concatenate the MSB and LSB } -#define PI 3.14159265359 -#define ON 0 -#define OFF 1 + +void initMag() { + char cmd[2]; + cmd[0] = MAG_CTRL_REG2; + cmd[1] = 0x80; + i2c.write(addr, cmd, 2); + + cmd[0] = MAG_CTRL_REG1; + cmd[1] = MAG_3110_SAMPLE80+MAG_3110_OVERSAMPLE2+MAG_3110_ACTIVE; // 0x91; + i2c.write(addr, cmd, 2); + } + + void calXY() //magnetometer calibration: finding max and min of X, Y axis { int tempXmax, tempXmin, tempYmax, tempYmin, newX, newY; redLed = ON; -// 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); - // Wait for slider to be pressed at one end > 30mm - while( tsi.readDistance() < 30 && tsi.readDistance() != 0 ) { + printf("Waiting for initial press\n"); + // Wait for slider to be pressed + while( tsi.readDistance() == 0 ) { + redLed = ON; + wait(0.2); redLed = OFF; wait(0.2); - redLed = ON; - wait(0.2); } + + printf("Waiting for release\n"); // Wait for release while( tsi.readDistance() != 0 ) { @@ -157,61 +182,37 @@ wait(0.2); } redLed = OFF; - wait(1.0); + wait(0.5); + + printf("Rotate\n"); 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(); + + while(tsi.readDistance() == 0) { + greenLed = ON; + wait(0.1); + greenLed = OFF; + wait(0.1); 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); + if (newX > tempXmax) tempXmax = newX; + if (newX < tempXmin) tempXmin = newX; + if (newY > tempYmax) tempYmax = newY; + if (newY < tempYmin) tempYmin = newY; } + 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)); + //store new X, Y values in EEPROM/Flash + // Calculate average from min/max avgX=(settings.maxX+settings.minX)/2; avgY=(settings.maxY+settings.minY)/2; - redLed = OFF; - // Wait for slider to be pressed at one end > 30mm - while( tsi.readDistance() < 30 && tsi.readDistance() != 0 ) { - greenLed = OFF; - wait(0.2); - greenLed = ON; - wait(0.2); - } - // Wait for release while( tsi.readDistance() != 0 ) { greenLed = OFF; @@ -222,35 +223,18 @@ greenLed = OFF; wait(1.0); -// 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 = OFF; greenLed = OFF; blueLed = OFF; - 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); + initMag(); // Get some values printf("DR_STATUS %X\n", readReg( MAG_DR_STATUS )); @@ -265,27 +249,11 @@ 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 = OFF; greenLed = OFF; blueLed = OFF; @@ -295,8 +263,8 @@ 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; - + + // Do something with heading - display direction and turn on blue LED if heading approx north if (abs(heading) <= 22.5) { printf("N\n"); blueLed = ON; } else blueLed = OFF; if (abs(heading) >= 157.5) printf("S\n"); if (heading >= 67.5 && heading <= 112.5) printf("E \n"); @@ -310,12 +278,5 @@ 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()); } }