Diff: main.cpp

Revision:

0:1fb00e911fe6

Child:

2:747b84e54088

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 12 14:39:25 2016 +0000
@@ -0,0 +1,422 @@
+#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;
+}
\ No newline at end of file