include the mbed library with this snippet

#include "mbed.h"
#include "Motor.h"
#include "rtos.h"
#include "ultrasonic.h"

//Account for time (approx. 2min, see serial output in terminal) for wi-fi server to set up before navigation code begins!!

Motor ma(p25, p14, p13);
Motor mb(p24, p11, p12); // pwm, fwd, rev
DigitalOut myled(LED1);
DigitalOut led4(LED4);
DigitalOut led3(LED3);

//speaker/sd set up
/*AnalogOut DACout(p18); //currently in pwm 26
wave_player waver(&DACout);
SDFileSystem sd(p5, p6, p7, p8, "sd");*/

Serial pc(USBTX, USBRX);
Serial esp(p28, p27); // tx, rx
DigitalOut reset(p26);
Timer t;

int  count,ended,timeout;
char buf[2024];
char snd[1024];
 
char ssid[32] = "iPhone";     // enter WiFi router ssid inside the quotes
char pwd [32] = "Jordan23#"; // enter WiFi router password inside the quotes
 
void SendCMD(),getreply(),ESPconfig(),ESPsetbaudrate();
 void dev_recv()
{
    myled = myled;
    while(esp.readable()) {
        pc.putc(esp.getc());
    }
}
 
void pc_recv()
{
    led4 = !led4;
    while(pc.readable()) {
        esp.putc(pc.getc());
    }
}
 

DigitalIn Enc1(p21); //right motor... 
DigitalIn Enc2(p22); //left motor
int oldEnc1 = 0; 
int oldEnc2 = 0;

int ticksR = 0; //# times right wheel encoder changed state
int ticksL = 0; //" "

void dist1(int distance)
{
    //put code here to execute when the distance has changed
    printf("Distance1 %d mm\r\n", distance);
}
void dist2(int distance2)
{
    //put code here to execute when the distance has changed
    printf("Distance2 %d mm\r\n", distance2);
}

//Sonar1 variables
ultrasonic mu1(p15, p16, .1, 1, &dist1);

//Sonar2 variables
ultrasonic mu2(p22, p21, .1, 1, &dist2);

void wheelEnc(void const *argument){
    while(1){
        if(Enc1 != oldEnc1) { // Increment ticks every time the state has switched. 
                ticksR++;
                oldEnc1 = Enc1;

            }
       if(Enc2 != oldEnc2) {
                ticksL++;
                oldEnc2 = Enc2;
            }
        pc.printf("Left ticks: %d \n", ticksL);
        pc.printf("Right ticks: %d \n", ticksR);
        }
    }
    
void sonarThread(void const *argument){
    while(1){
        mu1.checkDistance(); //front sonar
        mu2.checkDistance(); //right side sonar
        int muDist1 = mu1.getCurrentDistance();
        int muDist2 = mu2.getCurrentDistance();

        //always move straight forward
        ma.speed(-.8); //Left motor
        mb.speed(-.8); //Right motor
        myled =0;

        //Front sensor is not detecting a wall, but right side is too close
        if ((muDist1 > 250) && (muDist2 < 170)){
            ma.speed(-.2);
            mb.speed(-.9); 
        }
        //Front sensor is not detecting a wall, but right side is too far
        else if ((muDist1 > 250) && (muDist2 > 250)){
            ma.speed(-.6); //turn
            mb.speed(-0.4);
        }
        else if (muDist1 < 280){
                ma.speed(1); //
                mb.speed(-1);
                wait(0.3);
        }
        else if ((muDist1 > 250) && (muDist2 > 500)){
                ma.speed(-.8);
                mb.speed(-.8);
                wait(0.1);
                mb.speed(1);
                wait(0.1);
        }
        else {
        ma.speed(-.8);
        mb.speed(-.8);
        wait(.1);
        }
        }
    }
///////////////////////////
/*void audio_thread(void const *argument)
{
    ultra_mutex.lock();
    FILE *wave_file;
    wave_file=fopen("/sd/imperial_marchEdit.wav","r");
    waver.play(wave_file);
    fclose(wave_file);
    ultra_mutex.unlock();
}*/


int main()
{
    led3 = 1;
    wait(2);
    reset=0; //hardware reset for 8266
    pc.baud(9600);  // set what you want here depending on your terminal program speed
    pc.printf("\f\n\r-------------ESP8266 Hardware Reset-------------\n\r");
    wait(0.5);
    reset=1;
    timeout=2;
    getreply();
 
    esp.baud(9600);   // change this to the new ESP8266 baudrate if it is changed at any time.
 
    //ESPsetbaudrate();   //******************  include this routine to set a different ESP8266 baudrate  ******************
 
    ESPconfig();        //******************  include Config to set the ESP8266 configuration  ***********************
 
 
 
    pc.attach(&pc_recv, Serial::RxIrq);
    esp.attach(&dev_recv, Serial::RxIrq);
    //start measuring the distance
    mu1.startUpdates();
    mu2.startUpdates();
    
    Enc1.mode(PullUp); // requires a pullup resistor so i just used embed's feature.
    Enc2.mode(PullUp);
    
    Thread thread1(wheelEnc);
    Thread thread2(sonarThread);
    //Thread thread3(audio_thread);
    //Thread thread4(wifiThread);

    while(1) {
        
        Thread::wait(100);        

    }
}



///WIFI SERVER GUI SETUP

// Sets new ESP8266 baurate, change the esp.baud(xxxxx) to match your new setting once this has been executed
void ESPsetbaudrate()
{
    strcpy(snd, "AT+CIOBAUD=115200\r\n");   // change the numeric value to the required baudrate
    SendCMD();
}
 
//  +++++++++++++++++++++++++++++++++ This is for ESP8266 config only, run this once to set up the ESP8266 +++++++++++++++
void ESPconfig()
{

    wait(5);
    pc.printf("\f---------- Starting ESP Config ----------\r\n\n");
        strcpy(snd,".\r\n.\r\n");
    SendCMD();
        wait(1);
    pc.printf("---------- Reset & get Firmware ----------\r\n");
    strcpy(snd,"node.restart()\r\n");
    SendCMD();
    timeout=5;
    getreply();
    pc.printf(buf);
 
    wait(2);
 
    pc.printf("\n---------- Get Version ----------\r\n");
    strcpy(snd,"print(node.info())\r\n");
    SendCMD();
    timeout=4;
    getreply();
    pc.printf(buf);
 
    wait(3);
 
    // set CWMODE to 1=Station,2=AP,3=BOTH, default mode 1 (Station)
    pc.printf("\n---------- Setting Mode ----------\r\n");
    strcpy(snd, "wifi.setmode(wifi.STATION)\r\n");
    SendCMD();
    timeout=4;
    getreply();
    pc.printf(buf);
 
    wait(2);
 
   
 
    pc.printf("\n---------- Listing Access Points ----------\r\n");
    strcpy(snd, "function listap(t)\r\n");
        SendCMD();
        wait(1);
        strcpy(snd, "for k,v in pairs(t) do\r\n");
        SendCMD();
        wait(1);
        strcpy(snd, "print(k..\" : \"..v)\r\n");
        SendCMD();
        wait(1);
        strcpy(snd, "end\r\n");
        SendCMD();
        wait(1);
        strcpy(snd, "end\r\n");
        SendCMD();
        wait(1);
        strcpy(snd, "wifi.sta.getap(listap)\r\n");
    SendCMD();
    wait(1);
        timeout=15;
    getreply();
    pc.printf(buf);
 
    wait(2);
 
    pc.printf("\n---------- Connecting to AP ----------\r\n");
    pc.printf("ssid = %s   pwd = %s\r\n",ssid,pwd);
    strcpy(snd, "wifi.sta.config(\"");
    strcat(snd, ssid);
    strcat(snd, "\",\"");
    strcat(snd, pwd);
    strcat(snd, "\")\r\n");
    SendCMD();
    timeout=10;
    getreply();
    pc.printf(buf);
 
    wait(5);
 
    pc.printf("\n---------- Get IP's ----------\r\n");
    strcpy(snd, "print(wifi.sta.getip())\r\n");
    SendCMD();
    timeout=3;
    getreply();
    pc.printf(buf);
 
    wait(1);
 
    pc.printf("\n---------- Get Connection Status ----------\r\n");
    strcpy(snd, "print(wifi.sta.status())\r\n");
    SendCMD();
    timeout=5;
    getreply();
    pc.printf(buf);
 
    pc.printf("\n\n\n  If you get a valid (non zero) IP, ESP8266 has been set up.\r\n");
    pc.printf("  Run this if you want to reconfig the ESP8266 at any time.\r\n");
    pc.printf("  It saves the SSID and password settings internally\r\n");
    pc.printf(" Asha Harris and Teferi Lab 2 ");
    wait(10);
        
        
          pc.printf("\n---------- Setting up http server ----------\r\n");
    strcpy(snd, "srv=net.createServer(net.TCP)\r\n");
        SendCMD();
        wait(1);
        strcpy(snd, "srv:listen(80,function(conn)\r\n");
        SendCMD();
        wait(1);
        strcpy(snd, "conn:on(\"receive\",function(conn,payload)\r\n");
        SendCMD();
        wait(1);
        strcpy(snd, "print(payload)\r\n");
        SendCMD();
        wait(1);
        
        strcpy(snd, "conn:send(\"<!DOCTYPE html>\")\r\n");
        SendCMD();
      wait(1);
        
        strcpy(snd, "conn:send(\"<html>\")\r\n");
        SendCMD();
      wait(1);
        
        strcpy(snd, "conn:send(\"<h1> 4180 IOT Self Nav Robot </h1>\")\r\n");
      SendCMD();
        wait(1);
        
        sprintf(snd, "conn:send(\"%s'\")\r\n", ticksR);
        SendCMD();
        wait(1);
        
        strcpy(snd, "conn:send(\"<h2> cm.. </h2>\")\r\n");
        SendCMD();
        wait(1);
        
        strcpy(snd, "conn:send(\"</html>\")\r\n");
    SendCMD();
    wait(1);
        
        strcpy(snd, "end)\r\n");
    SendCMD();
    wait(1);
        
        strcpy(snd, "conn:on(\"sent\",function(conn) conn:close() end)\r\n");
    SendCMD();
    wait(1);
        strcpy(snd, "end)\r\n");
    SendCMD();
    wait(1);
        timeout=17;
    getreply();
    pc.printf(buf);
        pc.printf("\r\nDONE");
}
 
void SendCMD()
{
    esp.printf("%s", snd);
}
 
void getreply()
{
    memset(buf, '\0', sizeof(buf));
    t.start();
    ended=0;
    count=0;
    while(!ended) {
        if(esp.readable()) {
            buf[count] = esp.getc();
            count++;
        }
        if(t.read() > timeout) {
            ended = 1;
            t.stop();
            t.reset();
        }
    }
}