![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
A program using ESP8266 Huzzah to receive message from Amazon AWS service and control a servo to flip on/off the mechanical light switch.
Dependencies: RPCInterface Servo mbed
Fork of dotbot by
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