Andrew Lindsay / Mbed 2 deprecated Wi-Go-MagnetometerTest

Quick test of the Wi-Go Magnetometer

Dependencies:   TSI mbed MAG3110

main.cpp@3:945e32be0448, 2013-05-19 (annotated)

Committer:
SomeRandomBloke
Date:
Sun May 19 20:35:55 2013 +0000
Revision:
3:945e32be0448
Parent:
2:c19e63728e2e
Child:
4:e7831feff821
Move test code to a library

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 3:945e32be0448 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 3:945e32be0448 29 #include "MAG3110.h"
SomeRandomBloke 0:26bfff579f01 30
SomeRandomBloke 0:26bfff579f01 31
SomeRandomBloke 2:c19e63728e2e 32 #define PI 3.14159265359
SomeRandomBloke 2:c19e63728e2e 33 #define ON 0
SomeRandomBloke 2:c19e63728e2e 34 #define OFF 1
SomeRandomBloke 0:26bfff579f01 35
SomeRandomBloke 2:c19e63728e2e 36 // Some LEDs for showing status
SomeRandomBloke 0:26bfff579f01 37 DigitalOut redLed(LED_RED);
SomeRandomBloke 0:26bfff579f01 38 DigitalOut greenLed(LED_GREEN);
SomeRandomBloke 0:26bfff579f01 39 DigitalOut blueLed(LED_BLUE);
SomeRandomBloke 0:26bfff579f01 40
SomeRandomBloke 2:c19e63728e2e 41 // Slide sensor acts as a button
SomeRandomBloke 1:d45fc466a46e 42 TSISensor tsi;
SomeRandomBloke 2:c19e63728e2e 43
SomeRandomBloke 2:c19e63728e2e 44 // I2C used to communicate with sensor
SomeRandomBloke 0:26bfff579f01 45 I2C i2c(PTE0, PTE1);
SomeRandomBloke 0:26bfff579f01 46
SomeRandomBloke 3:945e32be0448 47 //MAG3100 mag(&i2c, &pc); //DEBUG verion
SomeRandomBloke 3:945e32be0448 48 MAG3110 mag(&i2c);
SomeRandomBloke 3:945e32be0448 49
SomeRandomBloke 3:945e32be0448 50 //int newX, tempXmin, tempXmax, newY, tempYmin, tempYmax;
SomeRandomBloke 2:c19e63728e2e 51 // Ideally these would be saved in eeprom/flash
SomeRandomBloke 3:945e32be0448 52 /*
SomeRandomBloke 0:26bfff579f01 53 struct settings_t {
SomeRandomBloke 0:26bfff579f01 54 long maxX, minX, maxY, minY;
SomeRandomBloke 0:26bfff579f01 55 }
SomeRandomBloke 0:26bfff579f01 56 settings;
SomeRandomBloke 3:945e32be0448 57 */
SomeRandomBloke 0:26bfff579f01 58
SomeRandomBloke 0:26bfff579f01 59
SomeRandomBloke 0:26bfff579f01 60 void calXY() //magnetometer calibration: finding max and min of X, Y axis
SomeRandomBloke 0:26bfff579f01 61 {
SomeRandomBloke 0:26bfff579f01 62 int tempXmax, tempXmin, tempYmax, tempYmin, newX, newY;
SomeRandomBloke 3:945e32be0448 63
SomeRandomBloke 1:d45fc466a46e 64 redLed = ON;
SomeRandomBloke 1:d45fc466a46e 65
SomeRandomBloke 2:c19e63728e2e 66 printf("Waiting for initial press\n");
SomeRandomBloke 2:c19e63728e2e 67 // Wait for slider to be pressed
SomeRandomBloke 2:c19e63728e2e 68 while( tsi.readDistance() == 0 ) {
SomeRandomBloke 2:c19e63728e2e 69 redLed = ON;
SomeRandomBloke 2:c19e63728e2e 70 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 71 redLed = OFF;
SomeRandomBloke 1:d45fc466a46e 72 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 73 }
SomeRandomBloke 3:945e32be0448 74
SomeRandomBloke 2:c19e63728e2e 75 printf("Waiting for release\n");
SomeRandomBloke 1:d45fc466a46e 76
SomeRandomBloke 1:d45fc466a46e 77 // Wait for release
SomeRandomBloke 1:d45fc466a46e 78 while( tsi.readDistance() != 0 ) {
SomeRandomBloke 1:d45fc466a46e 79 redLed = OFF;
SomeRandomBloke 1:d45fc466a46e 80 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 81 redLed = ON;
SomeRandomBloke 1:d45fc466a46e 82 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 83 }
SomeRandomBloke 1:d45fc466a46e 84 redLed = OFF;
SomeRandomBloke 2:c19e63728e2e 85 wait(0.5);
SomeRandomBloke 3:945e32be0448 86
SomeRandomBloke 2:c19e63728e2e 87 printf("Rotate\n");
SomeRandomBloke 3:945e32be0448 88
SomeRandomBloke 3:945e32be0448 89 tempXmax = tempXmin = mag.readVal(MAG_OUT_X_MSB);
SomeRandomBloke 3:945e32be0448 90 tempYmax = tempYmin = mag.readVal(MAG_OUT_Y_MSB);
SomeRandomBloke 2:c19e63728e2e 91
SomeRandomBloke 2:c19e63728e2e 92 while(tsi.readDistance() == 0) {
SomeRandomBloke 2:c19e63728e2e 93 greenLed = ON;
SomeRandomBloke 2:c19e63728e2e 94 wait(0.1);
SomeRandomBloke 2:c19e63728e2e 95 greenLed = OFF;
SomeRandomBloke 2:c19e63728e2e 96 wait(0.1);
SomeRandomBloke 3:945e32be0448 97 newX = mag.readVal(MAG_OUT_X_MSB);
SomeRandomBloke 3:945e32be0448 98 newY = mag.readVal(MAG_OUT_Y_MSB);
SomeRandomBloke 2:c19e63728e2e 99 if (newX > tempXmax) tempXmax = newX;
SomeRandomBloke 2:c19e63728e2e 100 if (newX < tempXmin) tempXmin = newX;
SomeRandomBloke 2:c19e63728e2e 101 if (newY > tempYmax) tempYmax = newY;
SomeRandomBloke 2:c19e63728e2e 102 if (newY < tempYmin) tempYmin = newY;
SomeRandomBloke 0:26bfff579f01 103 }
SomeRandomBloke 3:945e32be0448 104
SomeRandomBloke 3:945e32be0448 105 mag.setCalibration( tempXmin, tempXmax, tempYmin, tempYmax );
SomeRandomBloke 0:26bfff579f01 106
SomeRandomBloke 1:d45fc466a46e 107 // Wait for release
SomeRandomBloke 1:d45fc466a46e 108 while( tsi.readDistance() != 0 ) {
SomeRandomBloke 1:d45fc466a46e 109 greenLed = OFF;
SomeRandomBloke 1:d45fc466a46e 110 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 111 greenLed = ON;
SomeRandomBloke 1:d45fc466a46e 112 wait(0.2);
SomeRandomBloke 1:d45fc466a46e 113 }
SomeRandomBloke 1:d45fc466a46e 114 greenLed = OFF;
SomeRandomBloke 1:d45fc466a46e 115 wait(1.0);
SomeRandomBloke 1:d45fc466a46e 116
SomeRandomBloke 0:26bfff579f01 117
SomeRandomBloke 0:26bfff579f01 118 }
SomeRandomBloke 0:26bfff579f01 119
SomeRandomBloke 0:26bfff579f01 120 int main()
SomeRandomBloke 0:26bfff579f01 121 {
SomeRandomBloke 0:26bfff579f01 122 printf("MAG3110 Test\n");
SomeRandomBloke 0:26bfff579f01 123
SomeRandomBloke 1:d45fc466a46e 124 redLed = OFF;
SomeRandomBloke 1:d45fc466a46e 125 greenLed = OFF;
SomeRandomBloke 1:d45fc466a46e 126 blueLed = OFF;
SomeRandomBloke 3:945e32be0448 127
SomeRandomBloke 3:945e32be0448 128 mag.begin();
SomeRandomBloke 0:26bfff579f01 129
SomeRandomBloke 0:26bfff579f01 130
SomeRandomBloke 0:26bfff579f01 131 // Get some values
SomeRandomBloke 3:945e32be0448 132 printf("DR_STATUS %X\n", mag.readReg( MAG_DR_STATUS ));
SomeRandomBloke 3:945e32be0448 133 printf("WHO_AM_I %X\n", mag.readReg( MAG_WHO_AM_I ));
SomeRandomBloke 3:945e32be0448 134 printf("SYSMOD %X\n", mag.readReg( MAG_SYSMOD ));
SomeRandomBloke 3:945e32be0448 135 printf("DIE_TEMP %d\n", mag.readReg( MAG_DIE_TEMP ));
SomeRandomBloke 0:26bfff579f01 136
SomeRandomBloke 3:945e32be0448 137 printf("OFF_X %d\n", mag.readVal( MAG_OFF_X_MSB ));
SomeRandomBloke 3:945e32be0448 138 printf("OFF_Y %d\n", mag.readVal( MAG_OFF_Y_MSB ));
SomeRandomBloke 3:945e32be0448 139 printf("OFF_Z %d\n", mag.readVal( MAG_OFF_Z_MSB ));
SomeRandomBloke 0:26bfff579f01 140
SomeRandomBloke 3:945e32be0448 141 printf("CTRL_REG1 %X\n", mag.readReg( MAG_CTRL_REG1 ));
SomeRandomBloke 3:945e32be0448 142 printf("CTRL_REG2 %X\n", mag.readReg( MAG_CTRL_REG2 ));
SomeRandomBloke 0:26bfff579f01 143
SomeRandomBloke 0:26bfff579f01 144 printf("calibrate\n");
SomeRandomBloke 0:26bfff579f01 145 calXY();
SomeRandomBloke 0:26bfff579f01 146 printf("....Finished\n");
SomeRandomBloke 0:26bfff579f01 147
SomeRandomBloke 1:d45fc466a46e 148 redLed = OFF;
SomeRandomBloke 1:d45fc466a46e 149 greenLed = OFF;
SomeRandomBloke 1:d45fc466a46e 150 blueLed = OFF;
SomeRandomBloke 3:945e32be0448 151
SomeRandomBloke 0:26bfff579f01 152 while (1) {
SomeRandomBloke 0:26bfff579f01 153 wait(0.5);
SomeRandomBloke 3:945e32be0448 154 int xVal = mag.readVal(MAG_OUT_X_MSB);
SomeRandomBloke 3:945e32be0448 155 int yVal = mag.readVal(MAG_OUT_Y_MSB);
SomeRandomBloke 3:945e32be0448 156 float heading = mag.getHeading(); //(atan2((double)(yVal-avgY),(double)(xVal-avgX)))*180/PI;
SomeRandomBloke 3:945e32be0448 157
SomeRandomBloke 2:c19e63728e2e 158 // Do something with heading - display direction and turn on blue LED if heading approx north
SomeRandomBloke 3:945e32be0448 159 if (abs(heading) <= 22.5) {
SomeRandomBloke 3:945e32be0448 160 printf("N\n");
SomeRandomBloke 3:945e32be0448 161 blueLed = ON;
SomeRandomBloke 3:945e32be0448 162 } else blueLed = OFF;
SomeRandomBloke 0:26bfff579f01 163 if (abs(heading) >= 157.5) printf("S\n");
SomeRandomBloke 0:26bfff579f01 164 if (heading >= 67.5 && heading <= 112.5) printf("E \n");
SomeRandomBloke 0:26bfff579f01 165 if (heading <= -67.5 && heading >= -112.5) printf("W \n");
SomeRandomBloke 0:26bfff579f01 166 if (heading > 22.5 && heading < 67.5) printf("NE\n");
SomeRandomBloke 0:26bfff579f01 167 if (heading < -22.5 && heading > -67.5) printf("NW\n");
SomeRandomBloke 0:26bfff579f01 168 if (heading > 112.5 && heading < 157.5) printf("SE\n");
SomeRandomBloke 0:26bfff579f01 169 if (heading < -112.5 && heading > -157.5) printf("SW\n");
SomeRandomBloke 0:26bfff579f01 170
SomeRandomBloke 0:26bfff579f01 171 if (heading < 0) heading += 360.0;
SomeRandomBloke 0:26bfff579f01 172 printf("X = %d, Y = %d, Heading %f\n", xVal, yVal, heading);
SomeRandomBloke 0:26bfff579f01 173
SomeRandomBloke 0:26bfff579f01 174 }
SomeRandomBloke 0:26bfff579f01 175 }