Diff: main.cpp

Revision:

1:d45fc466a46e

Parent:

0:26bfff579f01

Child:

2:c19e63728e2e

--- a/main.cpp	Thu May 16 22:11:33 2013 +0000
+++ b/main.cpp	Fri May 17 11:56:00 2013 +0000
@@ -1,4 +1,5 @@
 #include "mbed.h"
+#include "TSISensor.h"
 #include "math.h"
 
 //PwmOut r (LED_RED);
@@ -80,6 +81,8 @@
 DigitalOut greenLed(LED_GREEN);
 DigitalOut blueLed(LED_BLUE);
 
+TSISensor tsi;
+   
 I2C i2c(PTE0, PTE1);
 
 int avgX, avgY, newX, tempXmin, tempXmax, newY, tempYmin, tempYmax;
@@ -121,12 +124,13 @@
 }
 
 #define PI 3.14159265359
-
+#define ON  0
+#define OFF 1
 
 void calXY() //magnetometer calibration: finding max and min of X, Y axis
 {
     int tempXmax, tempXmin, tempYmax, tempYmin, newX, newY;
-    redLed = 0;
+    redLed = ON;
 //    lcd.setCursor(0,1);
 //    lcd.print(“Rotate the car “);
 //    delay(1000);
@@ -136,6 +140,25 @@
 //    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 ) {
+        redLed = OFF;
+        wait(0.2);
+        redLed = ON;
+        wait(0.2);
+    }
+
+    // Wait for release
+    while( tsi.readDistance() != 0 ) {
+        redLed = OFF;
+        wait(0.2);
+        redLed = ON;
+        wait(0.2);
+    }
+    redLed = OFF;
+    wait(1.0);
+        
     tempXmax = tempXmin = readVal(MAG_OUT_X_MSB);
     tempYmax = tempYmin = readVal(MAG_OUT_Y_MSB);
     int changeCount = 10000;
@@ -180,7 +203,25 @@
     avgX=(settings.maxX+settings.minX)/2;
     avgY=(settings.maxY+settings.minY)/2;
 
-    redLed = 1;
+    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;
+        wait(0.2);
+        greenLed = ON;
+        wait(0.2);
+    }
+    greenLed = OFF;
+    wait(1.0);
+
 //    lcd.setCursor(0,1);
 //    lcd.print(“Calibration done”);
     //delay(3000);
@@ -198,9 +239,9 @@
 
     printf("MAG3110 Test\n");
 
-    redLed = 1;
-    greenLed = 1;
-    blueLed = 1;
+    redLed = OFF;
+    greenLed = OFF;
+    blueLed = OFF;
     
     cmd[0] = MAG_CTRL_REG2;
     cmd[1] = 0x80;
@@ -245,9 +286,9 @@
     printf("avgX = %d, avgY = %d\n", avgX, avgY);
 
  //   greenLed = 1;
-    redLed = 1;
-    greenLed = 1;
-    blueLed = 1;
+    redLed = OFF;
+    greenLed = OFF;
+    blueLed = OFF;
     
     while (1) {
         wait(0.5);
@@ -256,7 +297,7 @@
         float heading = (atan2((double)(yVal-avgY),(double)(xVal-avgX)))*180/PI;
 //        float heading = (atan2((double)(xVal-avgX),(double)(yVal-avgY)))*180/PI;
 
-        if (abs(heading) <= 22.5) { printf("N\n"); greenLed = 0; } else greenLed = 1;
+        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");
@@ -269,8 +310,6 @@
         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 ));