Prana Koirala / Mbed 2 deprecated Wifi_controlled_Robot_ECE4180_Lab4

Wifi Controlled Robot is done as a mini project (Lab4) for ECE 4180 class (Georgia Tech). Robot is assembled in Sparkfun's Shadow chasis robot kit. This robot takes the command from the webpage to move forward, reverse without colliding. The distance sensor is used as a mechanism for collision detection and play a short beep sound. It turn 90 degree with magnetometer when the turn command is sent.

Dependencies:   Motordriver mbed

/media/uploads/pkoirala3/capture.jpg Showing the setup and calibration: Part1 In this part the Wifi setups and attains an IP address so we can control it through a webpage on our phone/PC. After it obtains an IP address it calibrates the IMU so we can have accurate compass headings for precise turns.

The parts that we use are as follows:

  • Mbed
  • 2 DC motors
  • H-Bridge (to drive the DC motors)
  • Speaker
  • Class D Amp (to drive the Speaker)
  • IMU (use the magnometer for compass headings)
  • Wifi card (ESP8266)

Showing webpage functionality and Robot Functionality: In this part we demonstrate the functionality of the robot and the webpage to control it.

Final Code

[Repository '/teams/Prana-Koirala/code/Wifi_controlled_Robot_ECE4180/docs/tip/main_8cpp_source.html' not found]

Diff: main.cpp

Revision:
5:e9f1030a5bbb
Parent:
4:166570fa7fda
--- a/main.cpp	Mon Mar 13 17:53:47 2017 +0000
+++ b/main.cpp	Mon Mar 13 21:54:22 2017 +0000
@@ -8,15 +8,15 @@
 DigitalOut  ledFwd(p8);
 DigitalOut  ledRev(p11);
 
-Motor A(p26, p16, p25, 1); // pwm, fwd, rev, can brake
+Motor A(p26, p16, p15, 1); // pwm, fwd, rev, can brake
 Motor B(p21, p22, p23, 1); // pwm, fwd, rev, can brake
 
 Serial pc(USBTX, USBRX);
 Serial esp(p28, p27); // tx, rx
 
-Speaker mySpeaker(p24);
+Speaker mySpeaker(p18);
 
-// LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
+LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
 
 AnalogIn DistSensorFwd(p19);
 AnalogIn DistSensorRev(p20);
@@ -43,13 +43,15 @@
 char webbuff[4096];
 char stateBuff[50];
 void SendCMD(),getreply(),ReadWebData(),startserver();
-void ImuCheck(), ObjDetect(float);
+void ImuCheckLeft(), ImuCheckRight(), ObjDetect(float);
+float calcHeading(float, float, float);
 char rx_line[1024];
 int port = 80;  // set server port
 int SERVtimeout = 5;    // set server timeout in seconds in case link breaks.
 
 int main()
-{   
+{
+    mySpeaker.PlayNote(500.0,0.5,1.0);
     pc.baud(9600);
     esp.baud(9600);
     esp.attach(&Rx_interrupt, Serial::RxIrq); //serial interrupt to receive data
@@ -57,16 +59,16 @@
     startserver();
     wait(0.01);
     DataRX=0;
-
-// Initially stop the robot 
+    mySpeaker.PlayNote(500.0,0.5,1.0);
+// Initially stop the robot
     A.stop(0.0);
     B.stop(0.0);
-/*
+
     IMU.begin();
     IMU.calibrate(1);
     IMU.calibrateMag(0);
     pc.printf("Final Calibration done, Ready to move...\r\n");
-*/
+
     while(1) {
         if(DataRX==1) {
             ReadWebData();
@@ -78,8 +80,8 @@
         char temp_buff[50];
         if(abs(stateA) > 1  && abs(stateB) > 1) strcpy(temp_buff, "Robot Stopped!");
         else if((stateA > 0 && stateA <= 1)&&(stateB > 0 && stateB <= 1)) { // obstacle within 10 cm and robot is still moving
-            // float Dist = DistSensorFwd;
-            // ObjDetect(Dist);
+            float Dist = DistSensorFwd;
+            ObjDetect(Dist);
             strcpy(temp_buff, "Robot Moving Forward!");
         } else if((stateA < 0 && stateA >= -1)&&(stateB < 0 && stateB >= -1)) { // If moving rev
             float Dist = DistSensorRev;
@@ -136,18 +138,18 @@
         ledRev = 0;
     }
     if( strstr(webdata, "select=BotLeft") != NULL ) {
-        A.speed(0.5);
-        B.stop(-0.5);
-        // ImuCheck();
-        wait(1.0);
+        A.speed(0.6);
+        B.stop(-0.6);
+        ImuCheckLeft();
+        // wait(1.0);
         A.stop(0.0);
         B.stop(0.0);
     }
     if( strstr(webdata, "select=BotRight") != NULL ) {
-        A.speed(-0.5);
-        B.speed(0.5);
-        // ImuCheck();
-        wait(1.0);
+        A.speed(-0.6);
+        B.speed(0.6);
+        ImuCheckRight();
+        // wait(1.0);
         A.stop(0.0);
         B.stop(0.0);
     }
@@ -388,40 +390,57 @@
     return;
 }
 
-/*
-// To make Robot Turn approximately 90 degree using IMU mag
-void ImuCheck()
+void ImuCheckRight()
 {
-    bool rotFlag = true;
-    IMU.readMag();
-    float initial_heading;
-    float mx = -1*IMU.mx;
-    float my = IMU.my;
-    if(my == 0.0) initial_heading = (mx < 0.0) ? 180.0 : 0.0;
-    else initial_heading = atan2(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) {
+    float headingStart = calcHeading(IMU.calcMag(IMU.mx),IMU.calcMag(IMU.my),IMU.calcMag(IMU.mz));
+    float newHeading = headingStart + 90.0;
+    //pc.printf("Start Heading: %f degress\n\r",headingStart);
+    //pc.printf("New Heading  : %f degress\n\r",newHeading);
+    float heading = calcHeading(IMU.calcMag(IMU.mx),IMU.calcMag(IMU.my),IMU.calcMag(IMU.mz));
+    while (newHeading > heading) {
         IMU.readMag();
-        float mx = -1*IMU.mx;
-        float my = IMU.my;
-        float heading;
-        if (my == 0.0) heading = (mx < 0.0) ? 180.0 : 0.0;
-        else heading = atan2(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;
+        heading = calcHeading(IMU.calcMag(IMU.mx),IMU.calcMag(IMU.my),IMU.calcMag(IMU.mz));
+        if (heading < headingStart-5.0) heading = heading + 360.0;
+        //pc.printf("Magnetic Heading: %f degress\n\r",heading);
+        wait(0.1);
     }
 }
-*/
+
+
+void ImuCheckLeft()
+{
+    float headingStart = calcHeading(IMU.calcMag(IMU.mx),IMU.calcMag(IMU.my),IMU.calcMag(IMU.mz));
+    float newHeading = headingStart - 90.0;
+    //pc.printf("Start Heading: %f degress\n\r",headingStart);
+    //pc.printf("New Heading  : %f degress\n\r",newHeading);
+    float heading = calcHeading(IMU.calcMag(IMU.mx),IMU.calcMag(IMU.my),IMU.calcMag(IMU.mz));
+    while (newHeading < heading) {
+        IMU.readMag();
+        heading = calcHeading(IMU.calcMag(IMU.mx),IMU.calcMag(IMU.my),IMU.calcMag(IMU.mz));
+        if (heading > headingStart + 5.0) heading = heading - 360.0;
+        //pc.printf("Magnetic Heading: %f degress\n\r",heading);
+        wait(0.1);
+    }
+}
+
+float calcHeading(float mx, float my, float mz)
+{
+    mx = -mx;
+    float heading;
+    if (my == 0.0)
+        heading = (mx < 0.0) ? 180.0 : 0.0;
+    else
+        heading = atan2(mx, my)*360.0/(2.0*PI);
+    heading -= DECLINATION; //correct for geo location
+    if(heading>360.0) heading = heading - 360.0;
+    else if(heading<0.0) heading = 360.0  + heading;
+    return heading;
+}
 
 // Checking for obstacles in the way while moving Fwd and Rev
 void ObjDetect(float currDist)
 {
-    if(currDist > 0.98f) { // 1.0=4 cm, 0.0=30 cm
+    if(currDist > 0.9f) { // 1.0=4 cm, 0.0=30 cm
         myleds = 1;
         pc.printf("Obj detected at: %f cm\r\n",(1-currDist)*26+4);
         wait(0.001);
@@ -430,6 +449,7 @@
         ledFwd = 0;
         ledRev = 0;
         mySpeaker.PlayNote(500.0,0.5,0.5);
+        wait(1.0);
         myleds = 0;
     }
 }
\ No newline at end of file