Diff: main.cpp

Revision:

1:bbe16318d747

Parent:

0:553fab92a347

Child:

3:fff9904d2ecb

--- a/main.cpp	Sun Mar 12 16:32:11 2017 +0000
+++ b/main.cpp	Sun Mar 12 21:12:08 2017 +0000
@@ -1,6 +1,9 @@
 #include "mbed.h"
 #include "motordriver.h"
 #include "ultrasonic.h"
+#include "LSM9DS1.h"
+#define PI 3.14159
+#define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
 
 DigitalOut  ledFwd(p8);
 DigitalOut  ledRev(p9);
@@ -13,31 +16,12 @@
 
 PwmOut mySpeaker(p24);
 
-void dist(int);
-ultrasonic mu(p6, p7, .5, 2, &dist); //Check every .5 sec, timeout 1 sec,call dist when the distance changes
+LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
 
-void dist(int distance)
-{
-    if(distance < 1000) { // Distance is in the unit of cm
-        pc.printf("stop by sonar \r\n");
-        A.stop(0.5);
-        B.stop(0.5);
-        ledFwd = 0;
-        mySpeaker = 0.5; // Play Beep sound
-    }
-}
+AnalogIn DistSensorFwd(p19);
+AnalogIn DistSensorRev(p20);
 
-// some test values to show on web page
-AnalogIn DistSensor(p20);
-AnalogIn   Ain1(p18);
-AnalogIn   Ain2(p19);
-
-float temperature, AdcIn, Ht;
-float R1=100000, R2=10000; // resistor values to give a 10:1 reduction of measured AnalogIn voltage
-char Vcc[10];
-char Temp[10];
-
-// things for sending/receiving data over serial
+// variables for sending/receiving data over serial
 volatile int tx_in=0;
 volatile int tx_out=0;
 volatile int rx_in=0;
@@ -49,37 +33,35 @@
 void Rx_interrupt();
 void send_line();
 void read_line();
-
 int DataRX;
 int update;
-int count;
 char cmdbuff[1024];
 char replybuff[4096];
 char webdata[4096]; // This may need to be bigger depending on WEB browser used
 char webbuff[4096];     // Currently using 1986 characters, Increase this if more web page data added
-char timebuf[30];
 void SendCMD(),getreply(),ReadWebData(),startserver();
-void gettime(),setRTC(),gettemp(),getbattery();
+void ImuCheck(), ObsDetected(float);
 char rx_line[1024];
-int port        =80;  // set server port
-int SERVtimeout =5;    // set server timeout in seconds in case link breaks.
+int port = 80;  // set server port
+int SERVtimeout = 5;    // set server timeout in seconds in case link breaks.
 struct tm t;
 
 int main()
 {
     pc.baud(9600);
     esp.baud(9600);
-    ledFwd = 1;
-    // Setup a serial interrupt function to receive data
-    esp.attach(&Rx_interrupt, Serial::RxIrq);
-    // Setup a serial interrupt function to transmit data
-    esp.attach(&Tx_interrupt, Serial::TxIrq);
+    esp.attach(&Rx_interrupt, Serial::RxIrq); //serial interrupt to receive data
+    esp.attach(&Tx_interrupt, Serial::TxIrq); //serial interrupt to transmit data
     startserver();
     wait(0.01);
-    mu.startUpdates();   //start measuring the distance in sonar
     DataRX=0;
-    count=0;
+
     mySpeaker.period(1.0/500.0); // 500hz period
+
+    IMU.begin();
+    IMU.calibrate(1);
+    IMU.calibrateMag(0);
+
     while(1) {
         if(DataRX==1) {
             ReadWebData();
@@ -88,17 +70,14 @@
         float stateA = A.state();
         float stateB = B.state();
         if((stateA > 0 && stateA <= 1)&&(stateB > 0 && stateB <= 1)) { // obstacle within 10 cm and robot is still moving
-            mu.checkDistance();
+            // Add second distance Sensor code
+            float Dist = DistSensorFwd;
+            pc.printf("currDist: %f",Dist);
+            ObsDectected(Dist);
         } else if((stateA < 0 && stateA >= -1)&&(stateB < 0 && stateB >= -1)) { // If moving rev
-            float currDist = DistSensor;
-            pc.printf("currDist: %f",currDist);
-            if(currDist <= 0.5f) {
-                A.stop(0.5);
-                B.stop(0.5);
-                ledRev = 0;
-                mySpeaker = 0.5; // Play Beep sound
-
-            }
+            float Dist = DistSensorRev;
+            pc.printf("currDist: %f",Dist);
+            ObsDectected(Dist);
         }
     }
 }
@@ -136,11 +115,14 @@
     if( strstr(webdata, "check=BotLeft") != NULL ) {
         B.stop(0.5);
         A.speed(0.5);
-
+        ImuCheck();
+        A.stop(0.5);
     }
     if( strstr(webdata, "check=BotRight") != NULL ) {
         A.stop(0.5);
         B.speed(0.5);
+        ImuCheck();
+        B.stop(0.5);
     }
     if( strstr(webdata, "POST") != NULL ) { // set update flag if POST request
         update=1;
@@ -149,6 +131,7 @@
         update=1;
     }
 }
+
 // Starts webserver
 void startserver()
 {
@@ -269,7 +252,6 @@
     pc.printf("\n\nReady to go .....\r\n\n");
 }
 
-
 // ESP Command data send
 void SendCMD()
 {
@@ -340,7 +322,6 @@
     return;
 }
 
-
 // Interupt Routine to read in data from serial port
 void Rx_interrupt()
 {
@@ -366,4 +347,41 @@
         tx_out = (tx_out + 1) % buffer_size;
     }
     return;
+}
+
+// To make Robot Turn approximately 90 degree using IMU mag
+void ImuCheck()
+{
+    bool rotFlag = true;
+    IMU.readMag();
+    float initial_heading;
+    if(my == 0.0) initial_heading = (-1*mx < 0.0) ? 180.0 : 0.0;
+    else initial_heading = atan2(-1*mx,my)*360.0/(2.0*PI);
+    initial_heading -= DECLINATION;
+    if(initial_heading>180.0) initial_heading = initial_heading - 360.0;
+    else if(initial_heading<0) initial_heading += 360.0;
+
+    while(rotFlag) {
+        IMU.readMag();
+        float mx = -1*(imu.mx);
+        float heading;
+        if (my == 0.0) heading = (-1*mx < 0.0) ? 180.0 : 0.0;
+        else heading = atan2(-1*mx, my)*360.0/(2.0*PI);
+        heading -= DECLINATION; //correct for geo location
+        if(heading>180.0) heading = heading - 360.0;
+        else if(heading<0.0) heading += 360.0;
+        if(abs(heading - initial_heading) > 90) rotFlag = 1;
+    }
+}
+
+// Checking if object there is obstacles in the way( Fwd and Rev )
+void ObsDetected(float currDist)
+{
+    if(currDist <= 0.5f) {
+        A.stop(0.5);
+        B.stop(0.5);
+        ledFwd = 0;
+        ledRev = 0;
+        mySpeaker = 0.5; // Play Beep sound
+    }
 }
\ No newline at end of file