a robot you can talk to when you're lonely
Dependencies: HALLFX_ENCODER Motor RPCInterface mbed
main.cpp
- Committer:
- gmiles3
- Date:
- 2016-12-13
- Revision:
- 2:747b84e54088
- Parent:
- 0:1fb00e911fe6
File content as of revision 2:747b84e54088:
#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] = "DelosLobbyWifi"; char pwd[32] = "freezeallmotorfunctions"; char port[32] = "1035"; // must be port forwarded char timeout[32] = "28800"; // 28800 is max 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_out[0], 0, sizeof(rpc_out)); int length = (int)rx_line[3] - 48; // bytes 0 to 2 are trash; byte 3 is length of message if (length > 0 && length < 256) { for (int i = 0; i < length; i++) { rpc_in[i] = rx_line[i+4]; // bytes 4 to length+3 are the valid data } RPC::call(rpc_in, rpc_out); pc.printf("%s\n", rpc_out); } // lambda function is event-triggered and non-persistent // after it terminates, we need to close the existing connection and start another one 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; }