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
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 }
Generated on Thu Jul 14 2022 23:01:18 by
1.7.2
