File content as of revision 2:da3e288789a4:

// This mbed code creates a connection with the car mbeds server and reads from it
// The car will host the server, the remote controll will send data to the server

#include "mbed.h"
#include "PinDetect.h"
#include "SparkfunAnalogJoystick.h"
#include "LSM9DS1.h"
#define PI 3.14159

#define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
// Construct serial objects
Serial pc(USBTX, USBRX);
Serial esp(p28, p27); // tx, rx
// Standard Mbed LED definitions
DigitalOut  led1(LED1);
DigitalOut  led2(LED2);
// Joystick for control
SparkfunAnalogJoystick joystick(p18, p19, p20);
// Construct imu object
LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);//sda, scl
// function for geting mag heading
float calc_heading(float mx, float my);

// things 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 read_line();

int DataRX;
int update;
char cmdbuff[1024];
char replybuff[4096];
char webdata[4096];     // This may need to be bigger depending on WEB browser used
char webbuff[4096];     // Currently using 1986 characters, Increase this if more web page data added
void SendCMD();
//void getreply();
void getConnection();   // Sets up a connection with the car server
char rx_line[1024];
int port = 80;          // set server port
int SERVtimeout = 5;    // set server timeout in seconds in case link breaks.

int main()
{
    pc.baud(9600);
    esp.baud(9600);

    // Init leds
    led1=0,led2=0;
    
    // Setup a serial interrupt function to receive data
    esp.attach(&Rx_interrupt, Serial::RxIrq);
    // Setup a serial interrupt function to transmit data
    esp.attach(&Tx_interrupt, Serial::TxIrq);
    
    // Get connection to server
    getConnection();
    
    // Set up the imu
    IMU.begin();
    if (!IMU.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
    }
    IMU.calibrate(1);
    IMU.calibrateMag(0);
    float forward, current, diff;
    float servo_x;
    //initial heading calc, need to rotate by 360 degrees.
    while(!IMU.magAvailable(X_AXIS));
    IMU.readMag();
    forward = calc_heading(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my));
    
    // Variables used for joystick
    float lm, rm;
    float x, y;
    
    while (1) {
        // Update the motor speeds
            // Get x and y
            x = joystick.xAxis();
            y = joystick.yAxis();
            // Get the angle of direction
        // Determine which direction (straight, complete turn, or in a coordinate quadrant)
        // straight (set motor speed directly to y
        if (x < .1 && x > -.1) {
            lm = (y);
            rm = (-1*y);\
        }
        // Complete turn (set outside motor directly to x)
        else if ( y < .1 && y > -.1) {
            if (x > 0) {
                lm = (x);
                rm = (0);
            } else {
                lm = (0);
                rm = (x);  
            }  
        }
        
        // Update the servo   
        while(!IMU.magAvailable(X_AXIS));
        IMU.readMag();
        current = calc_heading(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my));
        // diff is the final difference from forward direction in [-180, 180] range
        // this needs to     be normalized to [0, 1] while only keeping [-45, 45]
        diff = fmod((forward - current + 180), 360) - 180;
        diff = (diff < -180) ? (diff + 360) : diff;
        servo_x = (diff / 90.0) + 0.45;
        
        // Send the updated value of the motors and servos
        sprintf(cmdbuff, "cl:send(\"<l=%f,r=%f,x=%f>\")\r\n)", lm, rm, servo_x);

        SendCMD();
        wait(0.18);
        led1=!led1;
    }
}

// Gets magnetic heading
float calc_heading(float mx, float my)
{
    // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
    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>180.0) heading = heading - 360.0;
    else if(heading<-180.0) heading = 360.0 + heading;
    else if(heading<0.0) heading = 360.0  + heading;

    // pc.printf("Magnetic Heading: %f degrees\n\r",heading);
    return heading;
}


// Sets up connection with car server
void getConnection()
{   
    // Reset the ESP8266
    pc.printf("++++++++++ Resetting ESP ++++++++++\r\n");
    strcpy(cmdbuff,"node.restart()\r\n");
    SendCMD();
    wait(1);
    
    // Disconncet from any potential connections
    strcpy(cmdbuff,"wifi.sta.disconnect()\r\n");
    SendCMD();
    wait(1);     
    // Set mode (STATION)
    strcpy(cmdbuff,"wifi.setmode(wifi.STATION)\r\n");
    SendCMD();
    //getreply();
    wait(1); 
    // Connect to car server
    strcpy(cmdbuff,"wifi.sta.config(\"Marlon's iPhone\", \"feelsbadman\")\r\n");
    SendCMD();
    wait(1);
    // Connect to server
    strcpy(cmdbuff,"wifi.sta.connect()\r\n");
    SendCMD();
    wait(1);
    
    strcpy(cmdbuff,"print(\"Looking for a connection\")\r\n");
    SendCMD();
    wait(1); 
    
    strcpy(cmdbuff,"tmr.alarm(1,2000,1, function()\r\n");
    SendCMD();
    wait(1); 
    
    strcpy(cmdbuff,"if(wifi.sta.getip()~=nil) then\r\n");
    SendCMD();
    wait(1); 
    
    strcpy(cmdbuff,"tmr.stop(1)\r\n");
    SendCMD();
    wait(1); 
    // Print out controller ip address
    strcpy(cmdbuff,"print(\"Connected!\")\r\n");
    SendCMD();
    wait(1); 
    // Print out controller ip address
    strcpy(cmdbuff,"print(\"Client IP Address: \",wifi.sta.getip())\r\n");
    SendCMD();
    wait(1);
    // Create connetion
    strcpy(cmdbuff,"cl=net.createConnection(net.TCP, 0)\r\n");
    SendCMD();
    wait(1);
    // Connect the connection
    strcpy(cmdbuff,"cl:connect(80, \"192.168.4.1\")\r\n");
    SendCMD();
    wait(1);
   
    strcpy(cmdbuff,"else\r\n");
    SendCMD();
    wait(1);
    
    strcpy(cmdbuff,"print(\"Connecting...\")\r\n");
    SendCMD();
    wait(1); 
    
    strcpy(cmdbuff,"end\r\n");
    SendCMD();
    wait(1);
    
    strcpy(cmdbuff,"end)\r\n");
    SendCMD();
    //getreply();
    wait(5); 
}


// 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);
//}

// FUNCTIONS BELOW ARE FOR RX AND TX INTERUPTS (NOT WEB STUFF)
 
// 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;
    //led3=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;
    }
    //led3=0;
    return;
}
 
 
// Interupt Routine to write out data to serial port
void Tx_interrupt() {
    //led2=1;
// 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;
    }
    //led2=0;
    return;
}