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