main.cpp

00001 #include "mbed.h"
00002 #include "motordriver.h"
00003 #include "LSM9DS1.h"
00004 #include "Speaker.h"
00005 #define PI 3.14159
00006 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
00007 
00008 DigitalOut  ledFwd(p8);
00009 DigitalOut  ledRev(p11);
00010 
00011 Motor A(p26, p16, p15, 1); // pwm, fwd, rev, can brake
00012 Motor B(p21, p22, p23, 1); // pwm, fwd, rev, can brake
00013 
00014 Serial pc(USBTX, USBRX);
00015 Serial esp(p28, p27); // tx, rx
00016 
00017 Speaker mySpeaker(p18);
00018 
00019 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
00020 
00021 AnalogIn DistSensorFwd(p19);
00022 AnalogIn DistSensorRev(p20);
00023 
00024 BusOut myleds(LED1, LED2, LED3, LED4);
00025 
00026 // variables for sending/receiving data over serial
00027 volatile int tx_in=0;
00028 volatile int tx_out=0;
00029 volatile int rx_in=0;
00030 volatile int rx_out=0;
00031 const int buffer_size = 4095;
00032 char tx_buffer[buffer_size+1];
00033 char rx_buffer[buffer_size+1];
00034 void Tx_interrupt();
00035 void Rx_interrupt();
00036 void send_line();
00037 void read_line();
00038 int DataRX;
00039 int update;
00040 char cmdbuff[1024];
00041 char replybuff[4096];
00042 char webdata[4096];
00043 char webbuff[4096];
00044 char stateBuff[50];
00045 void SendCMD(),getreply(),ReadWebData(),startserver();
00046 void ImuCheckLeft(), ImuCheckRight(), ObjDetect(float);
00047 float calcHeading(float, float, float);
00048 char rx_line[1024];
00049 int port = 80;  // set server port
00050 int SERVtimeout = 5;    // set server timeout in seconds in case link breaks.
00051 
00052 int main()
00053 {
00054     mySpeaker.PlayNote(500.0,0.5,1.0);
00055     pc.baud(9600);
00056     esp.baud(9600);
00057     esp.attach(&Rx_interrupt, Serial::RxIrq); //serial interrupt to receive data
00058     esp.attach(&Tx_interrupt, Serial::TxIrq); //serial interrupt to transmit data
00059     startserver();
00060     wait(0.01);
00061     DataRX=0;
00062     mySpeaker.PlayNote(500.0,0.5,1.0);
00063 // Initially stop the robot
00064     A.stop(0.0);
00065     B.stop(0.0);
00066 
00067     IMU.begin();
00068     IMU.calibrate(1);
00069     IMU.calibrateMag(0);
00070     pc.printf("Final Calibration done, Ready to move...\r\n");
00071 
00072     while(1) {
00073         if(DataRX==1) {
00074             ReadWebData();
00075             esp.attach(&Rx_interrupt, Serial::RxIrq);
00076         }
00077 
00078         float stateA = A.state();
00079         float stateB = B.state();
00080         char temp_buff[50];
00081         if(abs(stateA) > 1  && abs(stateB) > 1) strcpy(temp_buff, "Robot Stopped!");
00082         else if((stateA > 0 && stateA <= 1)&&(stateB > 0 && stateB <= 1)) { // obstacle within 10 cm and robot is still moving
00083             float Dist = DistSensorFwd;
00084             ObjDetect(Dist);
00085             strcpy(temp_buff, "Robot Moving Forward!");
00086         } else if((stateA < 0 && stateA >= -1)&&(stateB < 0 && stateB >= -1)) { // If moving rev
00087             float Dist = DistSensorRev;
00088             ObjDetect(Dist);
00089             strcpy(temp_buff, "Robot Moving Reverse!");
00090         } else if(abs(stateA) > 0 && abs(stateA) <= 1 && abs(stateB) > 1) {
00091             strcpy(temp_buff, "Robot turning right!");
00092         } else if(abs(stateB) > 0 && abs(stateB) <= 1 && abs(stateA) > 1) {
00093             strcpy(temp_buff, "Robot turning left!");
00094         } else strcpy(temp_buff, "Error getting Robot state");
00095 
00096         if(update==1) { // update the state of Robot in webpage
00097             sprintf(stateBuff, "%s",temp_buff);
00098             sprintf(cmdbuff, "BotState=\"%s\"\r\n",stateBuff);
00099             SendCMD();
00100             getreply();
00101             update=0;
00102         }
00103     }
00104 }
00105 
00106 // Reads and processes GET and POST web data
00107 void ReadWebData()
00108 {
00109     wait_ms(200);
00110     esp.attach(NULL,Serial::RxIrq);
00111     DataRX=0;
00112     memset(webdata, '\0', sizeof(webdata));
00113     strcpy(webdata, rx_buffer);
00114     memset(rx_buffer, '\0', sizeof(rx_buffer));
00115     rx_in = 0;
00116     rx_out = 0;
00117     // check web data for form information
00118     if( strstr(webdata, "select=BotFwd") != NULL ) {
00119         if(abs(A.state()) <= 1) A.stop(0.0);
00120         if(abs(B.state()) <= 1) B.stop(0.0);
00121         A.speed(0.75);
00122         B.speed(0.75);
00123         ledRev = 0;
00124         ledFwd = 1;
00125     }
00126     if( strstr(webdata, "select=BotRev") != NULL ) {
00127         if(abs(A.state()) <= 1) A.stop(0.0);
00128         if(abs(B.state()) <= 1) B.stop(0.0);
00129         A.speed(-0.75);
00130         B.speed(-0.75);
00131         ledFwd = 0;
00132         ledRev = 1;
00133     }
00134     if( strstr(webdata, "select=BotStop") != NULL ) {
00135         A.stop(0.0);
00136         B.stop(0.0);
00137         ledFwd = 0;
00138         ledRev = 0;
00139     }
00140     if( strstr(webdata, "select=BotLeft") != NULL ) {
00141         A.speed(0.6);
00142         B.stop(-0.6);
00143         ImuCheckLeft();
00144         // wait(1.0);
00145         A.stop(0.0);
00146         B.stop(0.0);
00147     }
00148     if( strstr(webdata, "select=BotRight") != NULL ) {
00149         A.speed(-0.6);
00150         B.speed(0.6);
00151         ImuCheckRight();
00152         // wait(1.0);
00153         A.stop(0.0);
00154         B.stop(0.0);
00155     }
00156     if( strstr(webdata, "POST") != NULL ) { // set update flag if POST request
00157         update=1;
00158     }
00159     if( strstr(webdata, "GET") != NULL && strstr(webdata, "favicon") == NULL ) { // set update flag for GET request but do not want to update for favicon requests
00160         update=1;
00161     }
00162 }
00163 
00164 // Starts webserver
00165 void startserver()
00166 {
00167     pc.printf("Resetting ESP Device....\r\n");
00168     strcpy(cmdbuff,"node.restart()\r\n");
00169     SendCMD();
00170     wait(2);
00171     getreply();
00172 
00173     pc.printf("\nStarting Server.....\r\n> ");
00174 
00175     // initial values
00176     sprintf(cmdbuff, "BotState=\"%s\"\r\n",stateBuff);
00177     SendCMD();
00178     getreply();
00179     wait(0.5);
00180 
00181     //create server
00182     sprintf(cmdbuff, "srv=net.createServer(net.TCP,%d)\r\n",SERVtimeout);
00183     SendCMD();
00184     getreply();
00185     wait(0.5);
00186     strcpy(cmdbuff,"srv:listen(80,function(conn)\r\n");
00187     SendCMD();
00188     getreply();
00189     wait(0.3);
00190     strcpy(cmdbuff,"conn:on(\"receive\",function(conn,payload) \r\n");
00191     SendCMD();
00192     getreply();
00193     wait(0.3);
00194 
00195     //print data to mbed
00196     strcpy(cmdbuff,"print(payload)\r\n");
00197     SendCMD();
00198     getreply();
00199     wait(0.2);
00200 
00201     //web page data
00202     strcpy(cmdbuff,"conn:send('<!DOCTYPE html><html><body><h1>ESP8266 Mbed IoT Web Controller</h1>')\r\n");
00203     SendCMD();
00204     getreply();
00205     wait(0.4);
00206 
00207     strcpy(cmdbuff,"conn:send('<br>Current State of Robot: '..BotState..'<br><hr>')\r\n");
00208     SendCMD();
00209     getreply();
00210     wait(0.2);
00211     strcpy(cmdbuff,"conn:send('<form method=\"POST\"')\r\n");
00212     SendCMD();
00213     getreply();
00214     wait(0.3);
00215     strcpy(cmdbuff, "conn:send('<p><input type=\"radio\" name=\"select\" value=\"BotFwd\"> Move Robot Forward')\r\n");
00216     SendCMD();
00217     getreply();
00218     wait(0.3);
00219     strcpy(cmdbuff, "conn:send('<p><input type=\"radio\" name=\"select\" value=\"BotRev\"> Move Robot Reverse')\r\n");
00220     SendCMD();
00221     getreply();
00222     wait(0.3);
00223     strcpy(cmdbuff, "conn:send('<p><input type=\"radio\" name=\"select\" value=\"BotStop\"> Stop Robot')\r\n");
00224     SendCMD();
00225     getreply();
00226     wait(0.3);
00227     strcpy(cmdbuff, "conn:send('<p><input type=\"radio\" name=\"select\" value=\"BotLeft\"> Turn Robot Left')\r\n");
00228     SendCMD();
00229     getreply();
00230     wait(0.3);
00231     strcpy(cmdbuff, "conn:send('<p><input type=\"radio\" name=\"select\" value=\"BotRight\"> Turn Robot Right')\r\n");
00232     SendCMD();
00233     getreply();
00234     wait(0.3);
00235     strcpy(cmdbuff,"conn:send('<p><input type=\"submit\" value=\"Send to Robot\"></form>')\r\n");
00236     SendCMD();
00237     getreply();
00238     wait(0.3);
00239     strcpy(cmdbuff, "conn:send('<p><h2>How to use:</h2><ul><li>Select a radiobutton </li><li>Click Send to Robot button</li></ul></body></html>')\r\n");
00240     SendCMD();
00241     getreply();
00242     wait(0.5);
00243     // end web page data
00244     strcpy(cmdbuff, "conn:on(\"sent\",function(conn) conn:close() end)\r\n"); // close current connection
00245     SendCMD();
00246     getreply();
00247     wait(0.3);
00248     strcpy(cmdbuff, "end)\r\n");
00249     SendCMD();
00250     getreply();
00251     wait(0.2);
00252     strcpy(cmdbuff, "end)\r\n");
00253     SendCMD();
00254     getreply();
00255     wait(0.2);
00256 
00257     strcpy(cmdbuff, "tmr.alarm(0, 1000, 1, function()\r\n");
00258     SendCMD();
00259     getreply();
00260     wait(0.2);
00261     strcpy(cmdbuff, "if wifi.sta.getip() == nil then\r\n");
00262     SendCMD();
00263     getreply();
00264     wait(0.2);
00265     strcpy(cmdbuff, "print(\"Connecting to AP...\\n\")\r\n");
00266     SendCMD();
00267     getreply();
00268     wait(0.2);
00269     strcpy(cmdbuff, "else\r\n");
00270     SendCMD();
00271     getreply();
00272     wait(0.2);
00273     strcpy(cmdbuff, "ip, nm, gw=wifi.sta.getip()\r\n");
00274     SendCMD();
00275     getreply();
00276     wait(0.2);
00277     strcpy(cmdbuff,"print(\"IP Address: \",ip)\r\n");
00278     SendCMD();
00279     getreply();
00280     wait(0.2);
00281     strcpy(cmdbuff,"tmr.stop(0)\r\n");
00282     SendCMD();
00283     getreply();
00284     wait(0.2);
00285     strcpy(cmdbuff,"end\r\n");
00286     SendCMD();
00287     getreply();
00288     wait(0.2);
00289     strcpy(cmdbuff,"end)\r\n");
00290     SendCMD();
00291     getreply();
00292     wait(0.2);
00293     pc.printf("\n\nReady to go .....\r\n\n");
00294 }
00295 
00296 // ESP Command data send
00297 void SendCMD()
00298 {
00299     int i;
00300     char temp_char;
00301     bool empty;
00302     i = 0;
00303 // Start Critical Section - don't interrupt while changing global buffer variables
00304     NVIC_DisableIRQ(UART1_IRQn);
00305     empty = (tx_in == tx_out);
00306     while ((i==0) || (cmdbuff[i-1] != '\n')) {
00307 // Wait if buffer full
00308         if (((tx_in + 1) % buffer_size) == tx_out) {
00309 // End Critical Section - need to let interrupt routine empty buffer by sending
00310             NVIC_EnableIRQ(UART1_IRQn);
00311             while (((tx_in + 1) % buffer_size) == tx_out) {
00312             }
00313 // Start Critical Section - don't interrupt while changing global buffer variables
00314             NVIC_DisableIRQ(UART1_IRQn);
00315         }
00316         tx_buffer[tx_in] = cmdbuff[i];
00317         i++;
00318         tx_in = (tx_in + 1) % buffer_size;
00319     }
00320     if (esp.writeable() && (empty)) {
00321         temp_char = tx_buffer[tx_out];
00322         tx_out = (tx_out + 1) % buffer_size;
00323 // Send first character to start tx interrupts, if stopped
00324         esp.putc(temp_char);
00325     }
00326 // End Critical Section
00327     NVIC_EnableIRQ(UART1_IRQn);
00328     return;
00329 }
00330 
00331 // Get Command and ESP status replies
00332 void getreply()
00333 {
00334     read_line();
00335     sscanf(rx_line,replybuff);
00336 }
00337 
00338 // Read a line from the large rx buffer from rx interrupt routine
00339 void read_line()
00340 {
00341     int i;
00342     i = 0;
00343 // Start Critical Section - don't interrupt while changing global buffer variables
00344     NVIC_DisableIRQ(UART1_IRQn);
00345 // Loop reading rx buffer characters until end of line character
00346     while ((i==0) || (rx_line[i-1] != '\r')) {
00347 // Wait if buffer empty
00348         if (rx_in == rx_out) {
00349 // End Critical Section - need to allow rx interrupt to get new characters for buffer
00350             NVIC_EnableIRQ(UART1_IRQn);
00351             while (rx_in == rx_out) {
00352             }
00353 // Start Critical Section - don't interrupt while changing global buffer variables
00354             NVIC_DisableIRQ(UART1_IRQn);
00355         }
00356         rx_line[i] = rx_buffer[rx_out];
00357         i++;
00358         rx_out = (rx_out + 1) % buffer_size;
00359     }
00360 // End Critical Section
00361     NVIC_EnableIRQ(UART1_IRQn);
00362     rx_line[i-1] = 0;
00363     return;
00364 }
00365 
00366 // Interupt Routine to read in data from serial port
00367 void Rx_interrupt()
00368 {
00369     DataRX=1;
00370 // Loop just in case more than one character is in UART's receive FIFO buffer
00371 // Stop if buffer full
00372     while ((esp.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
00373         rx_buffer[rx_in] = esp.getc();
00374 // Uncomment to Echo to USB serial to watch data flow
00375         pc.putc(rx_buffer[rx_in]);
00376         rx_in = (rx_in + 1) % buffer_size;
00377     }
00378     return;
00379 }
00380 
00381 // Interupt Routine to write out data to serial port
00382 void Tx_interrupt()
00383 {
00384 // Loop to fill more than one character in UART's transmit FIFO buffer
00385 // Stop if buffer empty
00386     while ((esp.writeable()) && (tx_in != tx_out)) {
00387         esp.putc(tx_buffer[tx_out]);
00388         tx_out = (tx_out + 1) % buffer_size;
00389     }
00390     return;
00391 }
00392 
00393 void ImuCheckRight()
00394 {
00395     float headingStart = calcHeading(IMU.calcMag(IMU.mx),IMU.calcMag(IMU.my),IMU.calcMag(IMU.mz));
00396     float newHeading = headingStart + 90.0;
00397     //pc.printf("Start Heading: %f degress\n\r",headingStart);
00398     //pc.printf("New Heading  : %f degress\n\r",newHeading);
00399     float heading = calcHeading(IMU.calcMag(IMU.mx),IMU.calcMag(IMU.my),IMU.calcMag(IMU.mz));
00400     while (newHeading > heading) {
00401         IMU.readMag();
00402         heading = calcHeading(IMU.calcMag(IMU.mx),IMU.calcMag(IMU.my),IMU.calcMag(IMU.mz));
00403         if (heading < headingStart-5.0) heading = heading + 360.0;
00404         //pc.printf("Magnetic Heading: %f degress\n\r",heading);
00405         wait(0.1);
00406     }
00407 }
00408 
00409 
00410 void ImuCheckLeft()
00411 {
00412     float headingStart = calcHeading(IMU.calcMag(IMU.mx),IMU.calcMag(IMU.my),IMU.calcMag(IMU.mz));
00413     float newHeading = headingStart - 90.0;
00414     //pc.printf("Start Heading: %f degress\n\r",headingStart);
00415     //pc.printf("New Heading  : %f degress\n\r",newHeading);
00416     float heading = calcHeading(IMU.calcMag(IMU.mx),IMU.calcMag(IMU.my),IMU.calcMag(IMU.mz));
00417     while (newHeading < heading) {
00418         IMU.readMag();
00419         heading = calcHeading(IMU.calcMag(IMU.mx),IMU.calcMag(IMU.my),IMU.calcMag(IMU.mz));
00420         if (heading > headingStart + 5.0) heading = heading - 360.0;
00421         //pc.printf("Magnetic Heading: %f degress\n\r",heading);
00422         wait(0.1);
00423     }
00424 }
00425 
00426 float calcHeading(float mx, float my, float mz)
00427 {
00428     mx = -mx;
00429     float heading;
00430     if (my == 0.0)
00431         heading = (mx < 0.0) ? 180.0 : 0.0;
00432     else
00433         heading = atan2(mx, my)*360.0/(2.0*PI);
00434     heading -= DECLINATION; //correct for geo location
00435     if(heading>360.0) heading = heading - 360.0;
00436     else if(heading<0.0) heading = 360.0  + heading;
00437     return heading;
00438 }
00439 
00440 // Checking for obstacles in the way while moving Fwd and Rev
00441 void ObjDetect(float currDist)
00442 {
00443     if(currDist > 0.9f) { // 1.0=4 cm, 0.0=30 cm
00444         myleds = 1;
00445         pc.printf("Obj detected at: %f cm\r\n",(1-currDist)*26+4);
00446         wait(0.001);
00447         A.stop(0.0);
00448         B.stop(0.0);
00449         ledFwd = 0;
00450         ledRev = 0;
00451         mySpeaker.PlayNote(500.0,0.5,0.5);
00452         wait(1.0);
00453         myleds = 0;
00454     }
00455 }