File content as of revision 0:1fb00e911fe6:

#include "mbed.h"
#include "Car.h"
#include "mywifi.h"
#include "Motor.h"
#include "HALLFX_ENCODER.h"
#include "mbed_rpc.h"

Serial pc(USBTX, USBRX);
Serial esp(p28, p27);
char ssid[32] = "ThugMansion";
char pwd[32] = "2paclives";
char port[32] = "1035"; // must be port forwarded
char timeout[32] = "28800";
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];
char cmdbuff[1024];
char rx_line[1024];


DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

float left_speed = .5;
float right_speed = .5;
float clicksTravelled;
Motor left_motor(p26,p24,p25);    // pwm, fwd, rev
Motor right_motor(p21, p23, p22); // pwm, fwd, rev
HALLFX_ENCODER right_hall(p13);    // left hall effect encoder
HALLFX_ENCODER left_hall(p14);   // left hall effect encoder
float encoderFactor = 40;
RPCFunction rpcTurnRight(&carTurnRight, "turnRight");
RPCFunction rpcTurnLeft(&carTurnLeft, "turnLeft");
RPCFunction rpcMoveForward(&carMoveForwardDistance, "moveForward");
RPCFunction rpcMoveBackward(&carMoveBackwardDistance, "moveBackward");
RPCFunction rpcTurn(&carTurn, "turn");

int main() {
    clicksTravelled = 0.0;
    pc.baud(9600);
    esp.baud(9600);
    led1=0,led2=0,led3=0,led4=0;
    esp.attach(&Rx_interrupt, Serial::RxIrq);
    esp.attach(&Tx_interrupt, Serial::TxIrq);
    wait(5);
    connectToNetwork();
    char rpc_in[256];
    char rpc_out[256];
    while (1) {
        getReply();
        memset(&rpc_in[0], 0, sizeof(rpc_in));
        memset(&rpc_in[0], 0, sizeof(rpc_out));
        int length = (int)rx_line[3] - 48;
        if (length > 0 && length < 256) {
            for (int i = 0; i < length; i++) {
                rpc_in[i] = rx_line[i+4];
            }
            RPC::call(rpc_in, rpc_out);
            pc.printf("%s\n", rpc_out);
        }
        strcpy(cmdbuff, "srv:close()\r\n");
        sendCMD();
        wait(.5);
        getReply();
        strcpy(cmdbuff, "srv=net.createServer(net.TCP,");
        strcat(cmdbuff, timeout);
        strcat(cmdbuff, ")\r\n");
        sendCMD();
        wait(.5);
        getReply();
        strcpy(cmdbuff, "srv:listen(");
        strcat(cmdbuff, port);
        strcat(cmdbuff, ",function(conn)\r\n");
        sendCMD();
        wait(.5);
        getReply();
        strcpy(cmdbuff, "conn:on(\"receive\", function(conn, payload) \r\n");
        sendCMD();
        wait(.5);
        getReply();
        strcpy(cmdbuff, "conn:send('");
        strcat(cmdbuff, reportStatus());
        strcat(cmdbuff, "')\r\n");
        sendCMD();
        wait(.5);
        getReply();
        strcpy(cmdbuff, "print(payload)\r\n");
        sendCMD();
        wait(.5);
        getReply();
        strcpy(cmdbuff, "end)\r\n");
        sendCMD();
        wait(.5);
        getReply();
        strcpy(cmdbuff, "end)\r\n");
        sendCMD();
        wait(.5);
        getReply();
    }
    
}







/* CAR FUNCTIONS */

void carStop() {
    left_motor.speed(0);
    right_motor.speed(0);
}

void carResetEncoders() {
    left_hall.reset();
    right_hall.reset();
    wait(0.1);
}

void carMoveForwardDistance(Arguments *in, Reply *out) {
    carResetEncoders();
    long diff;
    int dist = in->getArg<int>();
    float enc_dist = dist * encoderFactor;
    volatile float left_val= left_hall.read();
    volatile float right_val = right_hall.read();
    while (left_val < enc_dist || left_val < enc_dist) {
        left_val = left_hall.read();
        right_val = right_hall.read();
        diff = left_val - right_val;
        float k = 0.5;
        if (diff < 0) { // left has moved less than right
            left_motor.speed(left_speed);
            right_motor.speed(k*right_speed);
        } else if (diff > 0)  { // right has moved less than left
            left_motor.speed(k*left_speed);
            right_motor.speed(right_speed);
        } else {
            left_motor.speed(left_speed);
            right_motor.speed(right_speed);
        }
    }
    clicksTravelled += left_val;
    left_motor.speed(0);
    right_motor.speed(0);
}

void carMoveBackwardDistance(Arguments *in, Reply *out) {
    int dist = in->getArg<int>();
    if (dist > 100) {
        dist = 100;
    }
    carResetEncoders();
    long diff;
    float enc_dist = dist * encoderFactor;
    volatile float left_val= left_hall.read();
    volatile float right_val = right_hall.read();
    while (left_val < enc_dist || right_val < enc_dist) {
        left_val = left_hall.read();
        right_val = right_hall.read();
        diff = left_val-right_val;
        float k = 0.8;
        if (diff < 0) { // left has moved less than right
            left_motor.speed(-left_speed);
            right_motor.speed(-k*right_speed);
        } else if (diff > 0)  { // right has moved less than left
            left_motor.speed(-k*left_speed);
            right_motor.speed(-right_speed);
        } else {
            left_motor.speed(-left_speed);
            right_motor.speed(-right_speed);
        }
    }
    clicksTravelled += left_val;
    carStop();
}

void carTurn(Arguments *in, Reply *out) {
    carResetEncoders();
    float enc_dist = in->getArg<float>();
    volatile float left_val= left_hall.read();
    while (left_val < enc_dist) {
        left_val = left_hall.read();
        left_motor.speed(left_speed);
        right_motor.speed(-right_speed);
    }
    carStop();
}

void carTurnRight(Arguments *in, Reply *out) {
    carResetEncoders();
    float enc_dist = 170;
    volatile float left_val= left_hall.read();
    while (left_val < enc_dist) {
        left_val = left_hall.read();
        left_motor.speed(left_speed);
        right_motor.speed(-right_speed);
    }
    carStop();
    if (out != NULL) out->putData("GOT HERE");
}

void carTurnLeft(Arguments *in, Reply *out) {
    carResetEncoders();
    float enc_dist = 170;
    volatile float left_val= left_hall.read();
    while (left_val < enc_dist) {
        left_val = left_hall.read();
        left_motor.speed(-left_speed);
        right_motor.speed(right_speed);
    }
    carStop();
}










/* WIFI FUNCTIONS */

void connectToNetwork() {
    pc.printf("# Resetting ESP\r\n");
    strcpy(cmdbuff,"node.restart()\r\n");
    sendCMD();
    wait(5);
    getReply();

    led1=1,led2=0,led3=0;
    pc.printf("# Setting Mode\r\n");
    strcpy(cmdbuff, "wifi.setmode(wifi.STATION)\r\n");
    sendCMD();
    getReply();

    wait(2);
    led1=0,led2=1,led3=0;
    pc.printf("# Connecting to AP\r\n");
    pc.printf("# ssid = %s\t\tpwd = %s\r\n", ssid, pwd);
    strcpy(cmdbuff, "wifi.sta.config(\"");
    strcat(cmdbuff, ssid);
    strcat(cmdbuff, "\",\"");
    strcat(cmdbuff, pwd);
    strcat(cmdbuff, "\")\r\n");
    sendCMD();
    getReply();

    wait(2);
    led1=0,led2=0,led3=1;
    pc.printf("# Get IP Address\r\n");
    strcpy(cmdbuff, "print(wifi.sta.getip())\r\n");
    sendCMD();
    getReply();

    wait(2);
    led1=1,led2=0,led3=0;
    pc.printf("# Get Connection Status\r\n");
    strcpy(cmdbuff, "print(wifi.sta.status())\r\n");
    sendCMD();
    getReply();

    wait(2);
    led1=0,led2=1,led3=0;
    pc.printf("# Listen on Port\r\n");
    strcpy(cmdbuff, "srv=net.createServer(net.TCP,");
    strcat(cmdbuff, timeout);
    strcat(cmdbuff, ")\r\n");
    sendCMD();
    getReply();

    wait(2);
    led1=0,led2=0,led3=1;
    strcpy(cmdbuff, "srv:listen(");
    strcat(cmdbuff, port);
    strcat(cmdbuff, ",function(conn)\r\n");
    sendCMD();
    getReply();

    wait(2);
    led1=1,led2=0,led3=0;
    strcpy(cmdbuff, "conn:on(\"receive\", function(conn, payload) \r\n");
    sendCMD();
    getReply();

    wait(2);
    led1=0,led2=1,led3=0;
    strcpy(cmdbuff, "conn:send('");
    strcat(cmdbuff, reportStatus());
    strcat(cmdbuff, "')\r\n");
    sendCMD();
    getReply();

    wait(2);
    led1=0,led2=0,led3=1;
    strcpy(cmdbuff, "print(payload)\r\n");
    sendCMD();
    getReply();

    wait(2);
    led1=1,led2=0,led3=0;
    strcpy(cmdbuff, "end)\r\n");
    sendCMD();
    getReply();

    wait(2);
    led1=0,led2=1,led3=0;
    strcpy(cmdbuff, "end)\r\n");
    sendCMD();
    getReply();

    wait(2);
    //led1=1,led2=1,led3=1;
    led1=0,led2=0,led3=0;
    pc.printf("# Ready\r\n");
}

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

// Read a line from the large rx buffer from rx interrupt routine
void getReply() {
    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;
}

char* reportStatus() {
    char str[30];
    float inchesTravelled = clicksTravelled / encoderFactor ;
    sprintf(str, "%.1f", inchesTravelled);
    return str;
}

// Interupt Routine to read in data from serial port
void Rx_interrupt() {
    //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;
    }
    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;
}