Diff: main.cpp

Revision:

2:c19e63728e2e

Parent:

1:d45fc466a46e

Child:

3:945e32be0448

--- 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());
     }
 }