main.cpp

00001 #include "mbed.h"
00002 #include "Car.h"
00003 #include "mywifi.h"
00004 #include "Motor.h"
00005 #include "HALLFX_ENCODER.h"
00006 #include "mbed_rpc.h"
00007 
00008 Serial pc(USBTX, USBRX);
00009 Serial esp(p28, p27);
00010 char ssid[32] = "DelosLobbyWifi";
00011 char pwd[32] = "freezeallmotorfunctions";
00012 char port[32] = "1035"; // must be port forwarded
00013 char timeout[32] = "28800"; // 28800 is max
00014 volatile int tx_in=0;
00015 volatile int tx_out=0;
00016 volatile int rx_in=0;
00017 volatile int rx_out=0;
00018 const int buffer_size = 4095;
00019 char tx_buffer[buffer_size+1];
00020 char rx_buffer[buffer_size+1];
00021 char cmdbuff[1024];
00022 char rx_line[1024];
00023 
00024 
00025 DigitalOut led1(LED1);
00026 DigitalOut led2(LED2);
00027 DigitalOut led3(LED3);
00028 DigitalOut led4(LED4);
00029 
00030 float left_speed = .5;
00031 float right_speed = .5;
00032 float clicksTravelled;
00033 Motor left_motor(p26,p24,p25);    // pwm, fwd, rev
00034 Motor right_motor(p21, p23, p22); // pwm, fwd, rev
00035 HALLFX_ENCODER right_hall(p13);    // left hall effect encoder
00036 HALLFX_ENCODER left_hall(p14);   // left hall effect encoder
00037 float encoderFactor = 40;
00038 RPCFunction rpcTurnRight(&carTurnRight, "turnRight");
00039 RPCFunction rpcTurnLeft(&carTurnLeft, "turnLeft");
00040 RPCFunction rpcMoveForward(&carMoveForwardDistance, "moveForward");
00041 RPCFunction rpcMoveBackward(&carMoveBackwardDistance, "moveBackward");
00042 RPCFunction rpcTurn(&carTurn, "turn");
00043 
00044 int main() {
00045     clicksTravelled = 0.0;
00046     pc.baud(9600);
00047     esp.baud(9600);
00048     led1=0,led2=0,led3=0,led4=0;
00049     esp.attach(&Rx_interrupt, Serial::RxIrq);
00050     esp.attach(&Tx_interrupt, Serial::TxIrq);
00051     wait(5);
00052     connectToNetwork();
00053     char rpc_in[256];
00054     char rpc_out[256];
00055     while (1) {
00056         getReply();
00057         memset(&rpc_in[0], 0, sizeof(rpc_in));
00058         memset(&rpc_out[0], 0, sizeof(rpc_out));
00059         int length = (int)rx_line[3] - 48; // bytes 0 to 2 are trash; byte 3 is length of message
00060         if (length > 0 && length < 256) {
00061             for (int i = 0; i < length; i++) {
00062                 rpc_in[i] = rx_line[i+4]; // bytes 4 to length+3 are the valid data
00063             }
00064             RPC::call(rpc_in, rpc_out);
00065             pc.printf("%s\n", rpc_out);
00066         }
00067         // lambda function is event-triggered and non-persistent
00068         // after it terminates, we need to close the existing connection and start another one
00069         strcpy(cmdbuff, "srv:close()\r\n"); 
00070         sendCMD();
00071         wait(.5);
00072         getReply();
00073         strcpy(cmdbuff, "srv=net.createServer(net.TCP,");
00074         strcat(cmdbuff, timeout);
00075         strcat(cmdbuff, ")\r\n");
00076         sendCMD();
00077         wait(.5);
00078         getReply();
00079         strcpy(cmdbuff, "srv:listen(");
00080         strcat(cmdbuff, port);
00081         strcat(cmdbuff, ",function(conn)\r\n");
00082         sendCMD();
00083         wait(.5);
00084         getReply();
00085         strcpy(cmdbuff, "conn:on(\"receive\", function(conn, payload) \r\n");
00086         sendCMD();
00087         wait(.5);
00088         getReply();
00089         strcpy(cmdbuff, "conn:send('");
00090         strcat(cmdbuff, reportStatus());
00091         strcat(cmdbuff, "')\r\n");
00092         sendCMD();
00093         wait(.5);
00094         getReply();
00095         strcpy(cmdbuff, "print(payload)\r\n");
00096         sendCMD();
00097         wait(.5);
00098         getReply();
00099         strcpy(cmdbuff, "end)\r\n");
00100         sendCMD();
00101         wait(.5);
00102         getReply();
00103         strcpy(cmdbuff, "end)\r\n");
00104         sendCMD();
00105         wait(.5);
00106         getReply();
00107     }
00108     
00109 }
00110 
00111 
00112 
00113 
00114 
00115 
00116 
00117 /* CAR FUNCTIONS */
00118 
00119 void carStop() {
00120     left_motor.speed(0);
00121     right_motor.speed(0);
00122 }
00123 
00124 void carResetEncoders() {
00125     left_hall.reset();
00126     right_hall.reset();
00127     wait(0.1);
00128 }
00129 
00130 void carMoveForwardDistance(Arguments *in, Reply *out) {
00131     carResetEncoders();
00132     long diff;
00133     int dist = in->getArg<int>();
00134     float enc_dist = dist * encoderFactor;
00135     volatile float left_val= left_hall.read();
00136     volatile float right_val = right_hall.read();
00137     while (left_val < enc_dist || left_val < enc_dist) {
00138         left_val = left_hall.read();
00139         right_val = right_hall.read();
00140         diff = left_val - right_val;
00141         float k = 0.5;
00142         if (diff < 0) { // left has moved less than right
00143             left_motor.speed(left_speed);
00144             right_motor.speed(k*right_speed);
00145         } else if (diff > 0)  { // right has moved less than left
00146             left_motor.speed(k*left_speed);
00147             right_motor.speed(right_speed);
00148         } else {
00149             left_motor.speed(left_speed);
00150             right_motor.speed(right_speed);
00151         }
00152     }
00153     clicksTravelled += left_val;
00154     left_motor.speed(0);
00155     right_motor.speed(0);
00156 }
00157 
00158 void carMoveBackwardDistance(Arguments *in, Reply *out) {
00159     int dist = in->getArg<int>();
00160     if (dist > 100) {
00161         dist = 100;
00162     }
00163     carResetEncoders();
00164     long diff;
00165     float enc_dist = dist * encoderFactor;
00166     volatile float left_val= left_hall.read();
00167     volatile float right_val = right_hall.read();
00168     while (left_val < enc_dist || right_val < enc_dist) {
00169         left_val = left_hall.read();
00170         right_val = right_hall.read();
00171         diff = left_val-right_val;
00172         float k = 0.8;
00173         if (diff < 0) { // left has moved less than right
00174             left_motor.speed(-left_speed);
00175             right_motor.speed(-k*right_speed);
00176         } else if (diff > 0)  { // right has moved less than left
00177             left_motor.speed(-k*left_speed);
00178             right_motor.speed(-right_speed);
00179         } else {
00180             left_motor.speed(-left_speed);
00181             right_motor.speed(-right_speed);
00182         }
00183     }
00184     clicksTravelled += left_val;
00185     carStop();
00186 }
00187 
00188 void carTurn(Arguments *in, Reply *out) {
00189     carResetEncoders();
00190     float enc_dist = in->getArg<float>();
00191     volatile float left_val= left_hall.read();
00192     while (left_val < enc_dist) {
00193         left_val = left_hall.read();
00194         left_motor.speed(left_speed);
00195         right_motor.speed(-right_speed);
00196     }
00197     carStop();
00198 }
00199 
00200 void carTurnRight(Arguments *in, Reply *out) {
00201     carResetEncoders();
00202     float enc_dist = 170;
00203     volatile float left_val= left_hall.read();
00204     while (left_val < enc_dist) {
00205         left_val = left_hall.read();
00206         left_motor.speed(left_speed);
00207         right_motor.speed(-right_speed);
00208     }
00209     carStop();
00210     if (out != NULL) out->putData("GOT HERE");
00211 }
00212 
00213 void carTurnLeft(Arguments *in, Reply *out) {
00214     carResetEncoders();
00215     float enc_dist = 170;
00216     volatile float left_val= left_hall.read();
00217     while (left_val < enc_dist) {
00218         left_val = left_hall.read();
00219         left_motor.speed(-left_speed);
00220         right_motor.speed(right_speed);
00221     }
00222     carStop();
00223 }
00224 
00225 
00226 
00227 
00228 
00229 
00230 
00231 
00232 
00233 
00234 /* WIFI FUNCTIONS */
00235 
00236 void connectToNetwork() {
00237     pc.printf("# Resetting ESP\r\n");
00238     strcpy(cmdbuff,"node.restart()\r\n");
00239     sendCMD();
00240     wait(5);
00241     getReply();
00242 
00243     led1=1,led2=0,led3=0;
00244     pc.printf("# Setting Mode\r\n");
00245     strcpy(cmdbuff, "wifi.setmode(wifi.STATION)\r\n");
00246     sendCMD();
00247     getReply();
00248 
00249     wait(2);
00250     led1=0,led2=1,led3=0;
00251     pc.printf("# Connecting to AP\r\n");
00252     pc.printf("# ssid = %s\t\tpwd = %s\r\n", ssid, pwd);
00253     strcpy(cmdbuff, "wifi.sta.config(\"");
00254     strcat(cmdbuff, ssid);
00255     strcat(cmdbuff, "\",\"");
00256     strcat(cmdbuff, pwd);
00257     strcat(cmdbuff, "\")\r\n");
00258     sendCMD();
00259     getReply();
00260 
00261     wait(2);
00262     led1=0,led2=0,led3=1;
00263     pc.printf("# Get IP Address\r\n");
00264     strcpy(cmdbuff, "print(wifi.sta.getip())\r\n");
00265     sendCMD();
00266     getReply();
00267 
00268     wait(2);
00269     led1=1,led2=0,led3=0;
00270     pc.printf("# Get Connection Status\r\n");
00271     strcpy(cmdbuff, "print(wifi.sta.status())\r\n");
00272     sendCMD();
00273     getReply();
00274 
00275     wait(2);
00276     led1=0,led2=1,led3=0;
00277     pc.printf("# Listen on Port\r\n");
00278     strcpy(cmdbuff, "srv=net.createServer(net.TCP,");
00279     strcat(cmdbuff, timeout);
00280     strcat(cmdbuff, ")\r\n");
00281     sendCMD();
00282     getReply();
00283 
00284     wait(2);
00285     led1=0,led2=0,led3=1;
00286     strcpy(cmdbuff, "srv:listen(");
00287     strcat(cmdbuff, port);
00288     strcat(cmdbuff, ",function(conn)\r\n");
00289     sendCMD();
00290     getReply();
00291 
00292     wait(2);
00293     led1=1,led2=0,led3=0;
00294     strcpy(cmdbuff, "conn:on(\"receive\", function(conn, payload) \r\n");
00295     sendCMD();
00296     getReply();
00297 
00298     wait(2);
00299     led1=0,led2=1,led3=0;
00300     strcpy(cmdbuff, "conn:send('");
00301     strcat(cmdbuff, reportStatus());
00302     strcat(cmdbuff, "')\r\n");
00303     sendCMD();
00304     getReply();
00305 
00306     wait(2);
00307     led1=0,led2=0,led3=1;
00308     strcpy(cmdbuff, "print(payload)\r\n");
00309     sendCMD();
00310     getReply();
00311 
00312     wait(2);
00313     led1=1,led2=0,led3=0;
00314     strcpy(cmdbuff, "end)\r\n");
00315     sendCMD();
00316     getReply();
00317 
00318     wait(2);
00319     led1=0,led2=1,led3=0;
00320     strcpy(cmdbuff, "end)\r\n");
00321     sendCMD();
00322     getReply();
00323 
00324     wait(2);
00325     //led1=1,led2=1,led3=1;
00326     led1=0,led2=0,led3=0;
00327     pc.printf("# Ready\r\n");
00328 }
00329 
00330 void sendCMD()
00331 {
00332     int i;
00333     char temp_char;
00334     bool empty;
00335     i = 0;
00336 // Start Critical Section - don't interrupt while changing global buffer variables
00337     NVIC_DisableIRQ(UART1_IRQn);
00338     empty = (tx_in == tx_out);
00339     while ((i==0) || (cmdbuff[i-1] != '\n')) {
00340 // Wait if buffer full
00341         if (((tx_in + 1) % buffer_size) == tx_out) {
00342 // End Critical Section - need to let interrupt routine empty buffer by sending
00343             NVIC_EnableIRQ(UART1_IRQn);
00344             while (((tx_in + 1) % buffer_size) == tx_out) {
00345             }
00346 // Start Critical Section - don't interrupt while changing global buffer variables
00347             NVIC_DisableIRQ(UART1_IRQn);
00348         }
00349         tx_buffer[tx_in] = cmdbuff[i];
00350         i++;
00351         tx_in = (tx_in + 1) % buffer_size;
00352     }
00353     if (esp.writeable() && (empty)) {
00354         temp_char = tx_buffer[tx_out];
00355         tx_out = (tx_out + 1) % buffer_size;
00356 // Send first character to start tx interrupts, if stopped
00357         esp.putc(temp_char);
00358     }
00359 // End Critical Section
00360     NVIC_EnableIRQ(UART1_IRQn);
00361     return;
00362 }
00363 
00364 // Read a line from the large rx buffer from rx interrupt routine
00365 void getReply() {
00366     int i;
00367     i = 0;
00368 // Start Critical Section - don't interrupt while changing global buffer variables
00369     NVIC_DisableIRQ(UART1_IRQn);
00370 // Loop reading rx buffer characters until end of line character
00371     while ((i==0) || (rx_line[i-1] != '\r')) {
00372 // Wait if buffer empty
00373         if (rx_in == rx_out) {
00374 // End Critical Section - need to allow rx interrupt to get new characters for buffer
00375             NVIC_EnableIRQ(UART1_IRQn);
00376             while (rx_in == rx_out) {
00377             }
00378 // Start Critical Section - don't interrupt while changing global buffer variables
00379             NVIC_DisableIRQ(UART1_IRQn);
00380         }
00381         rx_line[i] = rx_buffer[rx_out];
00382         i++;
00383         rx_out = (rx_out + 1) % buffer_size;
00384     }
00385 // End Critical Section
00386     NVIC_EnableIRQ(UART1_IRQn);
00387     rx_line[i-1] = 0;
00388     return;
00389 }
00390 
00391 char* reportStatus() {
00392     char str[30];
00393     float inchesTravelled = clicksTravelled / encoderFactor ;
00394     sprintf(str, "%.1f", inchesTravelled);
00395     return str;
00396 }
00397 
00398 // Interupt Routine to read in data from serial port
00399 void Rx_interrupt() {
00400     //led3=1;
00401 // Loop just in case more than one character is in UART's receive FIFO buffer
00402 // Stop if buffer full
00403     while ((esp.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
00404         rx_buffer[rx_in] = esp.getc();
00405 // Uncomment to Echo to USB serial to watch data flow
00406         pc.putc(rx_buffer[rx_in]);
00407         rx_in = (rx_in + 1) % buffer_size;
00408     }
00409     return;
00410 }
00411 
00412 
00413 // Interupt Routine to write out data to serial port
00414 void Tx_interrupt() {
00415     //led2=1;
00416 // Loop to fill more than one character in UART's transmit FIFO buffer
00417 // Stop if buffer empty
00418     while ((esp.writeable()) && (tx_in != tx_out)) {
00419         esp.putc(tx_buffer[tx_out]);
00420         tx_out = (tx_out + 1) % buffer_size;
00421     }
00422     //led2=0;
00423     return;
00424 }