main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include "rtos.h"
00004 #include "ESP8266.h"
00005 #include "motordriver.h"
00006 
00007 /* Left and right motors */
00008 Motor motor_l(p22, p11, p12, 1); // pwm, fwd, rev, can brake
00009 Motor motor_r(p21, p13, p14, 1); // pwm, fwd, rev, can brake
00010 
00011 /* Virtual USB serial port */
00012 RawSerial pc(USBTX, USBRX); // tx, rx
00013 
00014 /* ESP8266 wifi chip */
00015 ESP8266 esp(p28, p27, p26, "SSID", "password"); // tx, rx, reset, SSID, password
00016 #define RCV_BUF_SIZE 1024
00017 char rcv[RCV_BUF_SIZE];
00018 volatile int rcv_tail = 0;
00019 volatile int rcv_head = 0;
00020     
00021 /* Speaker */
00022 PwmOut speaker(p24);
00023 
00024 /* Servo with IR sensor mounted */
00025 Servo servo_ir(p23);
00026 
00027 /* IR sensor */
00028 AnalogIn ir_sensor(p20);
00029 
00030 /* mbed LEDs */
00031 DigitalOut led1(LED1);
00032 DigitalOut led4(LED4);
00033 
00034 /* IR sensor thread */
00035 void ir_thread(void const * args) {
00036     while (1) {
00037         if (ir_sensor > 0.8) {
00038             speaker = 1;
00039         } else {
00040             speaker = 0;
00041         }
00042         Thread::wait(50);
00043     }
00044 }
00045 
00046 /* Circular buffer to hold commands */
00047 static int COM_BUF_SIZE = 100;
00048 volatile char * command_queue;
00049 volatile int queue_tail = 0;
00050 volatile int queue_head = 0;
00051 
00052 /* Command execute thread */
00053 void command_exec_thread(void const *args) {
00054     while (1) {
00055         if (queue_tail < queue_head) {
00056             /* Save comand in temporary char variable */
00057             char command = command_queue[queue_tail];
00058             
00059             /* Null the command consumed */
00060             command_queue[queue_tail] = '\0';
00061             
00062             /* Execute the command */
00063             switch(command) {
00064             case '\0': /* null command */
00065                 break;
00066             case 'F': /* Move forward */
00067                 motor_r.speed(1); 
00068                 motor_l.speed(1); 
00069                 Thread::wait(400);
00070                 motor_r.stop(1);
00071                 motor_l.stop(1);
00072                 break;
00073             case 'B': /* Move backwards */
00074                 motor_r.speed(-1); 
00075                 motor_l.speed(-1); 
00076                 Thread::wait(400);
00077                 motor_r.stop(1);
00078                 motor_l.stop(1);
00079                 break;
00080             case 'R': /* Move right */
00081                 motor_l.speed(1); 
00082                 Thread::wait(400);
00083                 motor_l.stop(1);
00084                 break;
00085             case 'L': /* Move left */
00086                 motor_r.speed(1); 
00087                 Thread::wait(400);
00088                 motor_r.stop(1);
00089                 break;
00090             case 'S': /* Turn servo right */
00091                 if (servo_ir > 0.0) {
00092                     float val = (servo_ir - .2);
00093                     servo_ir = val > 0.0 ? val : 0.0;
00094                 }
00095                 break;
00096             case 'P': /* Turn servo left */
00097                 if (servo_ir < 1.0) {
00098                     float val = (servo_ir + .2);
00099                     servo_ir = val < 1.0 ? val : 1.0;
00100                 }
00101                 break;
00102             default:
00103                 break;
00104             }
00105             /* Increment command index in circular buffer */
00106             queue_tail = (queue_tail + 1) % COM_BUF_SIZE;
00107         }
00108         Thread::wait(200);
00109     }
00110 }
00111  
00112  
00113 /* Queue command thread*/
00114 void command_queue_thread(void const *args) {
00115     while(1) {
00116         if (rcv_tail < rcv_head) {
00117             if (rcv[rcv_tail] == 'Z') {
00118                 /* Loop until valid value found */
00119                 while (rcv[(rcv_tail + 1) % RCV_BUF_SIZE] == '\0') {
00120                     Thread::wait(100);    
00121                 }
00122                 char let = rcv[(rcv_tail + 1) % RCV_BUF_SIZE];
00123                 command_queue[queue_head] = let;
00124                 queue_head = (queue_head + 1) % COM_BUF_SIZE;   
00125             }
00126             rcv[rcv_tail] = '\0'; /* Clear value */
00127             rcv_tail = (rcv_tail + 1) % RCV_BUF_SIZE;
00128         }
00129         Thread::wait(10);
00130     }
00131 }
00132     
00133 /* PC receive interrupt routine */
00134 void pc_recv() {
00135     led1 = 1;
00136     while(pc.readable()) {
00137         /* Print out to ESP8266 hardware */
00138         esp.putc(pc.getc());
00139     }
00140     led1 = 0;
00141 }
00142 
00143 /* ESP83266 receive interrupt routine */
00144 void dev_recv() {
00145     led4 = 1;
00146     while(esp.readable()) {
00147         char let = esp.getc();
00148         /* Echo to pc to see output */
00149         pc.putc(let);
00150         /* Put in receive buffer */
00151         rcv[rcv_head] = let;
00152         rcv_head = (rcv_head + 1) % RCV_BUF_SIZE;
00153     }
00154     led4 = 0;
00155 }
00156  
00157 /* Main code */
00158 int main() {
00159     /* Set pc serial baud rate */
00160     pc.baud(9600);
00161     
00162     /* Set up esp */
00163     esp.reset();
00164     esp.baud(9600);
00165     esp.setup();
00166     
00167     /* Initialize command buffer */
00168     command_queue = (char*) calloc(sizeof(char), COM_BUF_SIZE);
00169     
00170     /* Attach interrupts */
00171     pc.attach(&pc_recv, Serial::RxIrq);
00172     esp.attach(&dev_recv, Serial::RxIrq);
00173     
00174     /* Print IP and MAC */
00175     esp.getIP();
00176     esp.getMAC();
00177     
00178     /* Start threads */
00179     Thread command_exec_thr(command_exec_thread);
00180     Thread command_queue_thr(command_queue_thread);
00181     Thread ir_thr(ir_thread);
00182     
00183     while(1) {
00184         Thread::wait(10000);
00185     }
00186 }