Diff: main.cpp

Revision:

3:945e32be0448

Parent:

2:c19e63728e2e

Child:

4:e7831feff821

--- a/main.cpp	Sat May 18 10:00:03 2013 +0000
+++ b/main.cpp	Sun May 19 20:35:55 2013 +0000
@@ -19,76 +19,14 @@
  *
  * By Andrew D. Lindsay, @AndrewDLindsay
  *
- * 
+ *
  *
  */
 
 #include "mbed.h"
 #include "TSISensor.h"
 #include "math.h"
-
-#define MAG_ADDR 0x1D
-
-// define registers
-#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
+#include "MAG3110.h"
 
 
 #define PI 3.14159265359
@@ -106,61 +44,23 @@
 // I2C used to communicate with sensor
 I2C i2c(PTE0, PTE1);
 
-int avgX, avgY, newX, tempXmin, tempXmax, newY, tempYmin, tempYmax;
+//MAG3100 mag(&i2c, &pc); //DEBUG verion
+MAG3110 mag(&i2c);
+
+//int newX, tempXmin, tempXmax, newY, tempYmin, tempYmax;
 // Ideally these would be saved in eeprom/flash
+/*
 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
-}
-
-
-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;
 
     printf("Waiting for initial press\n");
@@ -171,7 +71,7 @@
         redLed = OFF;
         wait(0.2);
     }
-    
+
     printf("Waiting for release\n");
 
     // Wait for release
@@ -183,35 +83,26 @@
     }
     redLed = OFF;
     wait(0.5);
-    
+
     printf("Rotate\n");
-        
-    tempXmax = tempXmin = readVal(MAG_OUT_X_MSB);
-    tempYmax = tempYmin = readVal(MAG_OUT_Y_MSB);
+
+    tempXmax = tempXmin = mag.readVal(MAG_OUT_X_MSB);
+    tempYmax = tempYmin = mag.readVal(MAG_OUT_Y_MSB);
 
     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);
+        newX = mag.readVal(MAG_OUT_X_MSB);
+        newY = mag.readVal(MAG_OUT_Y_MSB);
         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;
-
-    //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;
+   
+    mag.setCalibration( tempXmin, tempXmax, tempYmin, tempYmax );
 
     // Wait for release
     while( tsi.readDistance() != 0 ) {
@@ -233,39 +124,42 @@
     redLed = OFF;
     greenLed = OFF;
     blueLed = OFF;
+
+    mag.begin();
     
-    initMag();
 
     // 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("DR_STATUS %X\n", mag.readReg( MAG_DR_STATUS ));
+    printf("WHO_AM_I %X\n", mag.readReg( MAG_WHO_AM_I ));
+    printf("SYSMOD %X\n", mag.readReg( MAG_SYSMOD ));
+    printf("DIE_TEMP %d\n", mag.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("OFF_X %d\n", mag.readVal( MAG_OFF_X_MSB ));
+    printf("OFF_Y %d\n", mag.readVal( MAG_OFF_Y_MSB ));
+    printf("OFF_Z %d\n", mag.readVal( MAG_OFF_Z_MSB ));
 
-    printf("CTRL_REG1 %X\n", readReg( MAG_CTRL_REG1 ));
-    printf("CTRL_REG2 %X\n", readReg( MAG_CTRL_REG2 ));
+    printf("CTRL_REG1 %X\n", mag.readReg( MAG_CTRL_REG1 ));
+    printf("CTRL_REG2 %X\n", mag.readReg( MAG_CTRL_REG2 ));
 
     printf("calibrate\n");
     calXY();
     printf("....Finished\n");
-    printf("avgX = %d, avgY = %d\n", avgX, avgY);
 
     redLed = OFF;
     greenLed = OFF;
     blueLed = OFF;
-    
+
     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;
-        
+        int xVal = mag.readVal(MAG_OUT_X_MSB);
+        int yVal = mag.readVal(MAG_OUT_Y_MSB);
+        float heading = mag.getHeading();       //(atan2((double)(yVal-avgY),(double)(xVal-avgX)))*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) <= 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");
         if (heading <= -67.5 && heading >= -112.5) printf("W \n");
@@ -275,7 +169,6 @@
         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);
 
     }