File content as of revision 5:e9f1030a5bbb:

#include "mbed.h"
#include "motordriver.h"
#include "LSM9DS1.h"
#include "Speaker.h"
#define PI 3.14159
#define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.

DigitalOut  ledFwd(p8);
DigitalOut  ledRev(p11);

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(p18);

LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);

AnalogIn DistSensorFwd(p19);
AnalogIn DistSensorRev(p20);

BusOut myleds(LED1, LED2, LED3, LED4);

// variables for sending/receiving data over serial
volatile int tx_in=0;
volatile int tx_out=0;
volatile int rx_in=0;
volatile int rx_out=0;
const int buffer_size = 4095;
char tx_buffer[buffer_size+1];
char rx_buffer[buffer_size+1];
void Tx_interrupt();
void Rx_interrupt();
void send_line();
void read_line();
int DataRX;
int update;
char cmdbuff[1024];
char replybuff[4096];
char webdata[4096];
char webbuff[4096];
char stateBuff[50];
void SendCMD(),getreply(),ReadWebData(),startserver();
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
    esp.attach(&Tx_interrupt, Serial::TxIrq); //serial interrupt to transmit data
    startserver();
    wait(0.01);
    DataRX=0;
    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();
            esp.attach(&Rx_interrupt, Serial::RxIrq);
        }

        float stateA = A.state();
        float stateB = B.state();
        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);
            strcpy(temp_buff, "Robot Moving Forward!");
        } else if((stateA < 0 && stateA >= -1)&&(stateB < 0 && stateB >= -1)) { // If moving rev
            float Dist = DistSensorRev;
            ObjDetect(Dist);
            strcpy(temp_buff, "Robot Moving Reverse!");
        } else if(abs(stateA) > 0 && abs(stateA) <= 1 && abs(stateB) > 1) {
            strcpy(temp_buff, "Robot turning right!");
        } else if(abs(stateB) > 0 && abs(stateB) <= 1 && abs(stateA) > 1) {
            strcpy(temp_buff, "Robot turning left!");
        } else strcpy(temp_buff, "Error getting Robot state");

        if(update==1) { // update the state of Robot in webpage
            sprintf(stateBuff, "%s",temp_buff);
            sprintf(cmdbuff, "BotState=\"%s\"\r\n",stateBuff);
            SendCMD();
            getreply();
            update=0;
        }
    }
}

// Reads and processes GET and POST web data
void ReadWebData()
{
    wait_ms(200);
    esp.attach(NULL,Serial::RxIrq);
    DataRX=0;
    memset(webdata, '\0', sizeof(webdata));
    strcpy(webdata, rx_buffer);
    memset(rx_buffer, '\0', sizeof(rx_buffer));
    rx_in = 0;
    rx_out = 0;
    // check web data for form information
    if( strstr(webdata, "select=BotFwd") != NULL ) {
        if(abs(A.state()) <= 1) A.stop(0.0);
        if(abs(B.state()) <= 1) B.stop(0.0);
        A.speed(0.75);
        B.speed(0.75);
        ledRev = 0;
        ledFwd = 1;
    }
    if( strstr(webdata, "select=BotRev") != NULL ) {
        if(abs(A.state()) <= 1) A.stop(0.0);
        if(abs(B.state()) <= 1) B.stop(0.0);
        A.speed(-0.75);
        B.speed(-0.75);
        ledFwd = 0;
        ledRev = 1;
    }
    if( strstr(webdata, "select=BotStop") != NULL ) {
        A.stop(0.0);
        B.stop(0.0);
        ledFwd = 0;
        ledRev = 0;
    }
    if( strstr(webdata, "select=BotLeft") != NULL ) {
        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.6);
        B.speed(0.6);
        ImuCheckRight();
        // wait(1.0);
        A.stop(0.0);
        B.stop(0.0);
    }
    if( strstr(webdata, "POST") != NULL ) { // set update flag if POST request
        update=1;
    }
    if( strstr(webdata, "GET") != NULL && strstr(webdata, "favicon") == NULL ) { // set update flag for GET request but do not want to update for favicon requests
        update=1;
    }
}

// Starts webserver
void startserver()
{
    pc.printf("Resetting ESP Device....\r\n");
    strcpy(cmdbuff,"node.restart()\r\n");
    SendCMD();
    wait(2);
    getreply();

    pc.printf("\nStarting Server.....\r\n> ");

    // initial values
    sprintf(cmdbuff, "BotState=\"%s\"\r\n",stateBuff);
    SendCMD();
    getreply();
    wait(0.5);

    //create server
    sprintf(cmdbuff, "srv=net.createServer(net.TCP,%d)\r\n",SERVtimeout);
    SendCMD();
    getreply();
    wait(0.5);
    strcpy(cmdbuff,"srv:listen(80,function(conn)\r\n");
    SendCMD();
    getreply();
    wait(0.3);
    strcpy(cmdbuff,"conn:on(\"receive\",function(conn,payload) \r\n");
    SendCMD();
    getreply();
    wait(0.3);

    //print data to mbed
    strcpy(cmdbuff,"print(payload)\r\n");
    SendCMD();
    getreply();
    wait(0.2);

    //web page data
    strcpy(cmdbuff,"conn:send('<!DOCTYPE html><html><body><h1>ESP8266 Mbed IoT Web Controller</h1>')\r\n");
    SendCMD();
    getreply();
    wait(0.4);

    strcpy(cmdbuff,"conn:send('<br>Current State of Robot: '..BotState..'<br><hr>')\r\n");
    SendCMD();
    getreply();
    wait(0.2);
    strcpy(cmdbuff,"conn:send('<form method=\"POST\"')\r\n");
    SendCMD();
    getreply();
    wait(0.3);
    strcpy(cmdbuff, "conn:send('<p><input type=\"radio\" name=\"select\" value=\"BotFwd\"> Move Robot Forward')\r\n");
    SendCMD();
    getreply();
    wait(0.3);
    strcpy(cmdbuff, "conn:send('<p><input type=\"radio\" name=\"select\" value=\"BotRev\"> Move Robot Reverse')\r\n");
    SendCMD();
    getreply();
    wait(0.3);
    strcpy(cmdbuff, "conn:send('<p><input type=\"radio\" name=\"select\" value=\"BotStop\"> Stop Robot')\r\n");
    SendCMD();
    getreply();
    wait(0.3);
    strcpy(cmdbuff, "conn:send('<p><input type=\"radio\" name=\"select\" value=\"BotLeft\"> Turn Robot Left')\r\n");
    SendCMD();
    getreply();
    wait(0.3);
    strcpy(cmdbuff, "conn:send('<p><input type=\"radio\" name=\"select\" value=\"BotRight\"> Turn Robot Right')\r\n");
    SendCMD();
    getreply();
    wait(0.3);
    strcpy(cmdbuff,"conn:send('<p><input type=\"submit\" value=\"Send to Robot\"></form>')\r\n");
    SendCMD();
    getreply();
    wait(0.3);
    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");
    SendCMD();
    getreply();
    wait(0.5);
    // end web page data
    strcpy(cmdbuff, "conn:on(\"sent\",function(conn) conn:close() end)\r\n"); // close current connection
    SendCMD();
    getreply();
    wait(0.3);
    strcpy(cmdbuff, "end)\r\n");
    SendCMD();
    getreply();
    wait(0.2);
    strcpy(cmdbuff, "end)\r\n");
    SendCMD();
    getreply();
    wait(0.2);

    strcpy(cmdbuff, "tmr.alarm(0, 1000, 1, function()\r\n");
    SendCMD();
    getreply();
    wait(0.2);
    strcpy(cmdbuff, "if wifi.sta.getip() == nil then\r\n");
    SendCMD();
    getreply();
    wait(0.2);
    strcpy(cmdbuff, "print(\"Connecting to AP...\\n\")\r\n");
    SendCMD();
    getreply();
    wait(0.2);
    strcpy(cmdbuff, "else\r\n");
    SendCMD();
    getreply();
    wait(0.2);
    strcpy(cmdbuff, "ip, nm, gw=wifi.sta.getip()\r\n");
    SendCMD();
    getreply();
    wait(0.2);
    strcpy(cmdbuff,"print(\"IP Address: \",ip)\r\n");
    SendCMD();
    getreply();
    wait(0.2);
    strcpy(cmdbuff,"tmr.stop(0)\r\n");
    SendCMD();
    getreply();
    wait(0.2);
    strcpy(cmdbuff,"end\r\n");
    SendCMD();
    getreply();
    wait(0.2);
    strcpy(cmdbuff,"end)\r\n");
    SendCMD();
    getreply();
    wait(0.2);
    pc.printf("\n\nReady to go .....\r\n\n");
}

// ESP Command data send
void SendCMD()
{
    int i;
    char temp_char;
    bool empty;
    i = 0;
// Start Critical Section - don't interrupt while changing global buffer variables
    NVIC_DisableIRQ(UART1_IRQn);
    empty = (tx_in == tx_out);
    while ((i==0) || (cmdbuff[i-1] != '\n')) {
// Wait if buffer full
        if (((tx_in + 1) % buffer_size) == tx_out) {
// End Critical Section - need to let interrupt routine empty buffer by sending
            NVIC_EnableIRQ(UART1_IRQn);
            while (((tx_in + 1) % buffer_size) == tx_out) {
            }
// Start Critical Section - don't interrupt while changing global buffer variables
            NVIC_DisableIRQ(UART1_IRQn);
        }
        tx_buffer[tx_in] = cmdbuff[i];
        i++;
        tx_in = (tx_in + 1) % buffer_size;
    }
    if (esp.writeable() && (empty)) {
        temp_char = tx_buffer[tx_out];
        tx_out = (tx_out + 1) % buffer_size;
// Send first character to start tx interrupts, if stopped
        esp.putc(temp_char);
    }
// End Critical Section
    NVIC_EnableIRQ(UART1_IRQn);
    return;
}

// Get Command and ESP status replies
void getreply()
{
    read_line();
    sscanf(rx_line,replybuff);
}

// Read a line from the large rx buffer from rx interrupt routine
void read_line()
{
    int i;
    i = 0;
// Start Critical Section - don't interrupt while changing global buffer variables
    NVIC_DisableIRQ(UART1_IRQn);
// Loop reading rx buffer characters until end of line character
    while ((i==0) || (rx_line[i-1] != '\r')) {
// Wait if buffer empty
        if (rx_in == rx_out) {
// End Critical Section - need to allow rx interrupt to get new characters for buffer
            NVIC_EnableIRQ(UART1_IRQn);
            while (rx_in == rx_out) {
            }
// Start Critical Section - don't interrupt while changing global buffer variables
            NVIC_DisableIRQ(UART1_IRQn);
        }
        rx_line[i] = rx_buffer[rx_out];
        i++;
        rx_out = (rx_out + 1) % buffer_size;
    }
// End Critical Section
    NVIC_EnableIRQ(UART1_IRQn);
    rx_line[i-1] = 0;
    return;
}

// Interupt Routine to read in data from serial port
void Rx_interrupt()
{
    DataRX=1;
// Loop just in case more than one character is in UART's receive FIFO buffer
// Stop if buffer full
    while ((esp.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
        rx_buffer[rx_in] = esp.getc();
// Uncomment to Echo to USB serial to watch data flow
        pc.putc(rx_buffer[rx_in]);
        rx_in = (rx_in + 1) % buffer_size;
    }
    return;
}

// Interupt Routine to write out data to serial port
void Tx_interrupt()
{
// Loop to fill more than one character in UART's transmit FIFO buffer
// Stop if buffer empty
    while ((esp.writeable()) && (tx_in != tx_out)) {
        esp.putc(tx_buffer[tx_out]);
        tx_out = (tx_out + 1) % buffer_size;
    }
    return;
}

void ImuCheckRight()
{
    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);
    }
}


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.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);
        A.stop(0.0);
        B.stop(0.0);
        ledFwd = 0;
        ledRev = 0;
        mySpeaker.PlayNote(500.0,0.5,0.5);
        wait(1.0);
        myleds = 0;
    }
}