Simple code to control a robot by sending it a string of commands over wifi

Dependencies:   Motordriver SDFileSystem mbed

Fork of HUZZAHESP8266-web-control-LPC1768 by Austin Dong

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "SDFileSystem.h"
00003 #include "motordriver.h"
00004 #include "Speaker.h"
00005 #include <string>
00006 Motor  right(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
00007 Motor left(p26, p25, p24, 1);
00008 Speaker mySpeaker(p18);
00009 DigitalIn Enc1(p19); //right motor
00010 DigitalIn Enc2(p20); //left motor
00011 Ticker Sampler; //Interrupt Routine to sample encoder ticks.
00012 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
00013 Serial pc(USBTX, USBRX);
00014 Serial esp(p13, p14); // tx, rx
00015 // Standard Mbed LED definitions
00016 DigitalOut  led1(LED1);
00017 DigitalOut  led2(LED2);
00018 DigitalOut  led3(LED3);
00019 DigitalOut  led4(LED4);
00020 int oldEnc1 = 0; //Was the encoder previously a 1 or zero?
00021 int oldEnc2 = 0;
00022 int ticksR = 0; //How many times has the Right wheel encoder changed state? 
00023 int ticksL = 0; //Same for left wheel
00024 int E; // the error  between the 2 wheel speeds
00025 float speedL =0; //PWM speed setting for left wheel (scaled between 0 and 1)
00026 float speedR=0; //Same for right wheel
00027 unsigned char Instr = 0;//What button was sent from app?
00028 unsigned char sentSpeed = 0; //What is the speed slider set to?
00029 // things for sending/receiving data over serial
00030 volatile int tx_in=0;
00031 volatile int tx_out=0;
00032 volatile int rx_in=0;
00033 volatile int rx_out=0;
00034 const int buffer_size = 4095;
00035 char tx_buffer[buffer_size+1];
00036 char rx_buffer[buffer_size+1];
00037 void Tx_interrupt();
00038 void Rx_interrupt();
00039 void send_line();
00040 void read_line();
00041 int DataRX;
00042 int update;
00043 int count;
00044 int start_path;
00045 char cmdbuff[1024];
00046 char replybuff[8192];
00047 char webdata[8192]; // This may need to be bigger depending on WEB browser used
00048 char webbuff[8192];     // Currently using 1986 characters, Increase this if more web page data added
00049 char timebuf[30];
00050 char rx_line[1024];
00051 int port        =80;  // set server port
00052 int SERVtimeout =5;    // set server timeout in seconds in case link breaks.
00053 struct tm t;
00054 
00055 // helper function declarations
00056 int charToNum(char);
00057 void SendCMD(),getreply(),ReadWebData(),startserver(),parsedata();sampleEncoder(); //Function for sampling and adjusting speeds. 
00058 
00059 
00060 int main()
00061 {
00062     pc.printf("ResetingSD Card!\n");   
00063    
00064     FILE *fp = fopen("/sd/path.txt", "w");
00065     if(fp == NULL) {
00066         error("Could not open file for write\n");
00067     }
00068     fclose(fp); 
00069     /// MOTOR STUFF
00070     Sampler.attach(&sampleEncoder, .02); //Sampler uses sampleEncoder function every 20ms
00071 
00072     Enc1.mode(PullUp); // requires a pullup resistor so i just used embed's feature.
00073     Enc2.mode(PullUp);
00074     /// WIFI STUFF
00075     pc.baud(9600);
00076     esp.baud(9600);
00077     led1=1,led2=0,led3=0, led4=0;
00078     // Setup a serial interrupt function to receive data
00079     esp.attach(&Rx_interrupt, Serial::RxIrq);
00080     // Setup a serial interrupt function to transmit data
00081     esp.attach(&Tx_interrupt, Serial::TxIrq);
00082     startserver();
00083     DataRX=0;
00084     count=0;
00085     start_path = 0;
00086     while(1) {
00087         if(DataRX==1) {
00088             ReadWebData();
00089             esp.attach(&Rx_interrupt, Serial::RxIrq);
00090         }
00091         if(update==1) 
00092         {
00093             SendCMD();
00094             getreply();
00095             update=0;   
00096         }
00097         if(start_path==1){// If data sent Start parsing data while running robot
00098             parsedata();
00099         }
00100     }
00101 }
00102 // convert an ASCII character of a number to the number it represents (NOT the ascii code of the number)
00103 // ex: '2' -> 2.
00104 int charToNum(char c) {
00105     return (int) (c - '0');    
00106 }
00107 
00108 // Reads and processes GET and POST web data
00109 void ReadWebData()
00110 {
00111     FILE *fp = fopen("/sd/path.txt", "w");
00112     if(fp == NULL) {
00113         error("Could not open file for write\n");
00114     }
00115     fclose(fp); 
00116     wait_ms(200);
00117     esp.attach(NULL,Serial::RxIrq);
00118     DataRX=0;
00119     memset(webdata, '\0', sizeof(webdata));
00120     strcpy(webdata, rx_buffer);
00121     memset(rx_buffer, '\0', sizeof(rx_buffer));
00122     rx_in = 0;
00123     rx_out = 0;
00124     // check web data for form information write  info to sd card
00125     if( strstr(webdata, "d1=") != NULL ) {
00126         led2=!led2;
00127         pc.printf(strstr(webdata, "d1"));
00128         string dirs = strstr(webdata, "d1");
00129         string str2, str3;
00130         size_t pos;
00131         pos = dirs.find("stop")-2;    // position of "stop" in str
00132         str3 = dirs.substr (3,pos);   // get from "stop" to the end
00133         const char *mycharp = str3.c_str();
00134         pc.printf(mycharp);
00135         fp = fopen("/sd/path.txt", "a"); //Open "speedlog.txt" on the local file system for appending ('a' switch)
00136         fprintf(fp,mycharp); //write value and newline
00137         fclose(fp); //close file
00138         start_path = 1;
00139     }
00140     if( strstr(webdata, "POST") != NULL ) { // set update flag if POST request
00141         update=1;
00142     }
00143     if( strstr(webdata, "GET") != NULL && strstr(webdata, "favicon") == NULL ) { // set update flag for GET request but do not want to update for favicon requests
00144         update=1;
00145     }
00146 
00147 }
00148 // Starts webserver
00149 void parsedata(){
00150     pc.printf("\n");
00151     pc.printf("+++++++++ROBOT+RUNNING\n");
00152     unsigned char m;
00153     const char f= 'f';
00154     const char l= 'l';
00155     const char r='r';
00156     const char b='b';
00157     const char stop='s';
00158     int dur;
00159     
00160     
00161     FILE *fp = fopen("/sd/path.txt", "r");
00162     while (!feof(fp)){
00163     dur = 0;
00164     m=fgetc(fp);
00165     if (m==f){
00166         dur = charToNum(fgetc(fp));
00167         pc.printf("Forward for %i seconds\n",dur);
00168         speedL = float(100) / 100;
00169         speedR = speedL;
00170         left.speed(speedL);
00171         right.speed(speedR);
00172         Instr = 1;
00173     }
00174     if (m==b){
00175         dur = charToNum(fgetc(fp));
00176         pc.printf("Backward for %i seconds\n",dur);
00177         speedL = -float(50) / 100;
00178         speedR = speedL;
00179         left.speed(speedL);
00180         right.speed(speedR);
00181         Instr = 2;
00182     }
00183     if (m==r){
00184         dur = charToNum(fgetc(fp));
00185         pc.printf("Right for %i seconds\n",dur);
00186         speedL = float(50) / 100;
00187         left.speed(speedL);
00188         right.speed(-speedL);
00189     }
00190     if (m==l){
00191         dur = charToNum(fgetc(fp));
00192         pc.printf("Left for %i seconds\n",dur);
00193         speedL = float(50) / 100;
00194         left.speed(-speedL);
00195         right.speed(speedL);
00196     }
00197     if (m==stop){
00198         printf("WOOT \n");
00199         pc.printf("Putting on the Brakes\n");
00200         left.speed(0);
00201         right.speed(0);
00202         mySpeaker.PlayNote(440.0, 0.5, 0.5);
00203     }
00204     if(Instr == 1 || Instr == 2) { // Only increment tick values if moving forward or reverse
00205             if(Enc1 != oldEnc1) { // Increment ticks every time the state has switched. 
00206                 ticksR++;
00207                 oldEnc1 = Enc1;
00208 
00209             }
00210             if(Enc2 != oldEnc2) {
00211                 ticksL++;
00212                 oldEnc2 = Enc2;
00213             }
00214     }
00215     wait(dur);
00216     left.speed(0);
00217     right.speed(0);
00218     wait(.1);
00219     }
00220     fclose(fp); //close file
00221     update=0;
00222     start_path=0;
00223     }
00224 
00225 
00226 void sampleEncoder()
00227 {
00228     if((Instr == 1 || Instr == 2) && ticksL != 0) { //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped. 
00229 
00230         E = ticksL - ticksR; //Find error
00231         speedR = speedR + float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
00232         if(Instr == 1) speedR = speedR + float(E) / 255.0f;
00233         right.speed(speedR);
00234     }
00235     ticksR = 0; //Restart the counters
00236     ticksL = 0;
00237 }
00238 
00239 void startserver()
00240 {
00241 
00242     pc.printf("++++++++++ Resetting ESP ++++++++++\r\n");
00243     strcpy(cmdbuff,"node.restart()\r\n");
00244     SendCMD();
00245     wait(2);
00246     getreply();
00247     
00248     pc.printf("\n++++++++++ Starting Server ++++++++++\r\n> ");
00249 
00250 
00251     //create server
00252     sprintf(cmdbuff, "srv=net.createServer(net.TCP,%d)\r\n",SERVtimeout);
00253     SendCMD();
00254     getreply();
00255     wait(0.5);
00256     strcpy(cmdbuff,"srv:listen(80,function(conn)\r\n");
00257     SendCMD();
00258     getreply();
00259     wait(0.3);
00260         strcpy(cmdbuff,"conn:on(\"receive\",function(conn,payload) \r\n");
00261         SendCMD();
00262         getreply();
00263         wait(0.3);
00264         
00265         //print data to mbed
00266         strcpy(cmdbuff,"print(payload)\r\n");
00267         SendCMD();
00268         getreply();
00269         wait(0.2);
00270        
00271         //web page data
00272         strcpy(cmdbuff,"conn:send('<!DOCTYPE html><html><body><h1>ESP8266 Mbed IoT Web Controller</h1>')\r\n");
00273         SendCMD();
00274         getreply();
00275         wait(0.4);
00276         
00277         strcpy(cmdbuff,"conn:send('<form method=\"POST\"')\r\n");
00278         SendCMD();
00279         getreply();
00280         wait(0.3);
00281         strcpy(cmdbuff, "conn:send('<p>Directions:<input type=\"text\" name=\"d1\" value=\"f2l3f1stop\">')\r\n");
00282         SendCMD();
00283         getreply();
00284         wait(0.3);
00285         strcpy(cmdbuff,"conn:send('<p><input type=\"submit\" value=\"send-refresh\"></form>')\r\n");
00286         SendCMD();
00287         getreply();
00288         wait(0.3);
00289         
00290         strcpy(cmdbuff, "conn:send('<p><h2>How to use:</h2><ul><li>Build path in basedon value form</li><li>Click Send-Refresh to send data and refresh values</li></ul></body></html>')\r\n");
00291         SendCMD();
00292         getreply();
00293         wait(0.5); 
00294         // end web page data
00295         strcpy(cmdbuff, "conn:on(\"sent\",function(conn) conn:close() end)\r\n"); // close current connection
00296         SendCMD();
00297         getreply();
00298         wait(0.3);
00299         strcpy(cmdbuff, "end)\r\n");
00300         SendCMD();
00301         getreply();
00302         wait(0.2);
00303     strcpy(cmdbuff, "end)\r\n");
00304     SendCMD();
00305     getreply();
00306     wait(0.2);
00307 
00308     strcpy(cmdbuff, "tmr.alarm(0, 1000, 1, function()\r\n");
00309     SendCMD();
00310     getreply();
00311     wait(0.2);
00312     strcpy(cmdbuff, "if wifi.sta.getip() == nil then\r\n");
00313     SendCMD();
00314     getreply();
00315     wait(0.2);
00316     strcpy(cmdbuff, "print(\"Connecting to AP...\\n\")\r\n");
00317     SendCMD();
00318     getreply();
00319     wait(0.2);
00320     strcpy(cmdbuff, "else\r\n");
00321     SendCMD();
00322     getreply();
00323     wait(0.2);
00324     strcpy(cmdbuff, "ip, nm, gw=wifi.sta.getip()\r\n");
00325     SendCMD();
00326     getreply();
00327     wait(0.2);
00328     strcpy(cmdbuff,"print(\"IP Address: \",ip)\r\n");
00329     SendCMD();
00330     getreply();
00331     wait(0.2);
00332     strcpy(cmdbuff,"tmr.stop(0)\r\n");
00333     SendCMD();
00334     getreply();
00335     wait(0.2);
00336     strcpy(cmdbuff,"end\r\n");
00337     SendCMD();
00338     getreply();
00339     wait(0.2);
00340     strcpy(cmdbuff,"end)\r\n");
00341     SendCMD();
00342     getreply();
00343     wait(0.2);
00344     strcpy(cmdbuff,"end)\r\n");
00345     SendCMD();
00346     getreply();
00347     wait(0.2);
00348     
00349     pc.printf("\n\n++++++++++ Ready ++++++++++\r\n\n");
00350 }
00351 
00352 
00353 // ESP Commands 
00354 void SendCMD()
00355 {
00356     int i;
00357     char temp_char;
00358     bool empty;
00359     i = 0;
00360 // Start Critical Section - don't interrupt while changing global buffer variables
00361     NVIC_DisableIRQ(UART1_IRQn);
00362     empty = (tx_in == tx_out);
00363     while ((i==0) || (cmdbuff[i-1] != '\n')) {
00364 // Wait if buffer full
00365         if (((tx_in + 1) % buffer_size) == tx_out) {
00366 // End Critical Section - need to let interrupt routine empty buffer by sending
00367             NVIC_EnableIRQ(UART1_IRQn);
00368             while (((tx_in + 1) % buffer_size) == tx_out) {
00369             }
00370 // Start Critical Section - don't interrupt while changing global buffer variables
00371             NVIC_DisableIRQ(UART1_IRQn);
00372         }
00373         tx_buffer[tx_in] = cmdbuff[i];
00374         i++;
00375         tx_in = (tx_in + 1) % buffer_size;
00376     }
00377     if (esp.writeable() && (empty)) {
00378         temp_char = tx_buffer[tx_out];
00379         tx_out = (tx_out + 1) % buffer_size;
00380 // Send first character to start tx interrupts, if stopped
00381         esp.putc(temp_char);
00382     }
00383 // End Critical Section
00384     NVIC_EnableIRQ(UART1_IRQn);
00385     return;
00386 }
00387 
00388 // Get Command and ESP status replies
00389 void getreply()
00390 {
00391     read_line();
00392     sscanf(rx_line,replybuff);
00393 }
00394  
00395 // Read a line from the large rx buffer from rx interrupt routine
00396 void read_line() {
00397     int i;
00398     i = 0;
00399 // Start Critical Section - don't interrupt while changing global buffer variables
00400     NVIC_DisableIRQ(UART1_IRQn);
00401 // Loop reading rx buffer characters until end of line character
00402     while ((i==0) || (rx_line[i-1] != '\r')) {
00403 // Wait if buffer empty
00404         if (rx_in == rx_out) {
00405 // End Critical Section - need to allow rx interrupt to get new characters for buffer
00406             NVIC_EnableIRQ(UART1_IRQn);
00407             while (rx_in == rx_out) {
00408             }
00409 // Start Critical Section - don't interrupt while changing global buffer variables
00410             NVIC_DisableIRQ(UART1_IRQn);
00411         }
00412         rx_line[i] = rx_buffer[rx_out];
00413         i++;
00414         rx_out = (rx_out + 1) % buffer_size;
00415     }
00416 // End Critical Section
00417     NVIC_EnableIRQ(UART1_IRQn);
00418     rx_line[i-1] = 0;
00419     return;
00420 }
00421  
00422  
00423 // Interupt Routine to read in data from serial port
00424 void Rx_interrupt() {
00425     DataRX=1;
00426     //led3=1;
00427 // Loop just in case more than one character is in UART's receive FIFO buffer
00428 // Stop if buffer full
00429     while ((esp.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
00430         rx_buffer[rx_in] = esp.getc();
00431 // Uncomment to Echo to USB serial to watch data flow
00432         pc.putc(rx_buffer[rx_in]);
00433         rx_in = (rx_in + 1) % buffer_size;
00434     }
00435     //led3=0;
00436     return;
00437 }
00438  
00439  
00440 // Interupt Routine to write out data to serial port
00441 void Tx_interrupt() {
00442     //led2=1;
00443 // Loop to fill more than one character in UART's transmit FIFO buffer
00444 // Stop if buffer empty
00445     while ((esp.writeable()) && (tx_in != tx_out)) {
00446         esp.putc(tx_buffer[tx_out]);
00447         tx_out = (tx_out + 1) % buffer_size;
00448     }
00449     //led2=0;
00450     return;
00451 }