Diff: main.cpp

Revision:

6:a51f4ac6f42b

Parent:

4:18e9f16cc53c

Child:

7:cbccf5c5da6d

--- a/main.cpp	Sun Apr 10 22:03:16 2016 +0000
+++ b/main.cpp	Sun Apr 24 21:08:30 2016 +0000
@@ -4,15 +4,17 @@
 #define DECLINATION -4.94
 #define PI 3.14159
 
-
+// Timer example: https://developer.mbed.org/handbook/Timer
 AnalogIn infraredR(p20); //Right infrared distance sensor
 AnalogIn infraredL(p19); //Left infrared distance sensor
 AnalogIn infraredF(p17); //Front infrared distance sensor
 Serial pc(USBTX, USBRX);
-
+Timer t;
+    
 float leftDist, rightDist, frontDist; //Distances from robot to obstacles
 float xaccel, yaccel, zaccel;   //Acceleration in the x, y, and z directions
 float xmag, ymag, zmag, head;         //Magnetic readings
+int ldist, rdist, fdist;
 
 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
 {
@@ -44,6 +46,10 @@
 
 void IMU_thread(void const *args) {
     LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
+    IMU.begin();
+    if (!IMU.begin()) {
+        pc.printf("Failed to communicate with LSM9DS1.\n");
+    }
     IMU.calibrate(1);
     IMU.calibrateMag(0);
     while(1) {
@@ -54,6 +60,7 @@
         xaccel = IMU.calcAccel(IMU.ax);
         yaccel = IMU.calcAccel(IMU.ay);
         zaccel = IMU.calcAccel(IMU.az);
+        pc.printf("%f\r\n",zaccel);
         xmag = IMU.calcMag(IMU.mx);
         ymag = IMU.calcMag(IMU.my);
         zmag = IMU.calcMag(IMU.mz);
@@ -64,31 +71,42 @@
 
 void left_thread(void const *args) {
     while (1) {
-        leftDist = infraredL * 3.3f;
+        leftDist = infraredL * 80.0f;  
+        leftDist = 80.0f - leftDist;
+        ldist = (int)leftDist;
         Thread::wait(500);     //wait 1/2 second before updating
+        //pc.printf("Left distance %d\r\n", ldist);
         }
 }
 
 void right_thread(void const *args) {
     while (1) {
-        rightDist = infraredR * 3.3f;
+        rightDist = infraredR * 80.0f;
+        rightDist = 80.0f - rightDist;
+        rdist = (int)rightDist;
+        rdist = (int)rightDist;
         Thread::wait(500);     //wait 1/2 second before updating
+        //pc.printf("Right distance %d\r\n", rdist);
         }
 }
 
 void front_thread(void const *args) {
     while (1) {
-        frontDist = infraredF * 3.3f;
+        frontDist = infraredF * 80.0f;
+        frontDist = 80.0f - frontDist;
+        fdist = (int)frontDist;
         Thread::wait(500);     //wait 1/2 second before updating
+        pc.printf("Front distance %d\r\n", fdist);
         }
 }    
 
 int main() {
     //Test
+    t.begin();
     Thread left(left_thread);
     Thread right(right_thread);
     Thread front(front_thread);
-    
+    Thread IMU(IMU_thread);
     while(1) {
         
     }