Kairi Kozuma / ESP8266_wifi_robot

Wifi controlled robot that uses ESP8266 wifi chip.

Dependencies:   Motordriver Servo mbed-dev mbed-rtos

Fork of ESP8266_wifi_robot by Kairi Kozuma

main.cpp@0:df754b773321, 2016-10-29 (annotated)

Committer:
K2Silver
Date:
Sat Oct 29 15:35:26 2016 +0000
Revision:
0:df754b773321
Initial commit of wifi controlled robot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
K2Silver 0:df754b773321 1 #include "mbed.h"
K2Silver 0:df754b773321 2 #include "Servo.h"
K2Silver 0:df754b773321 3 #include "rtos.h"
K2Silver 0:df754b773321 4 #include "ESP8266.h"
K2Silver 0:df754b773321 5 #include "motordriver.h"
K2Silver 0:df754b773321 6
K2Silver 0:df754b773321 7 /* Left and right motors */
K2Silver 0:df754b773321 8 Motor motor_l(p22, p11, p12, 1); // pwm, fwd, rev, can brake
K2Silver 0:df754b773321 9 Motor motor_r(p21, p13, p14, 1); // pwm, fwd, rev, can brake
K2Silver 0:df754b773321 10
K2Silver 0:df754b773321 11 /* Virtual USB serial port */
K2Silver 0:df754b773321 12 RawSerial pc(USBTX, USBRX); // tx, rx
K2Silver 0:df754b773321 13
K2Silver 0:df754b773321 14 /* ESP8266 wifi chip */
K2Silver 0:df754b773321 15 ESP8266 esp(p28, p27, p26, "SSID", "password"); // tx, rx, reset, SSID, password
K2Silver 0:df754b773321 16 #define RCV_BUF_SIZE 1024
K2Silver 0:df754b773321 17 char rcv[RCV_BUF_SIZE];
K2Silver 0:df754b773321 18 volatile int rcv_tail = 0;
K2Silver 0:df754b773321 19 volatile int rcv_head = 0;
K2Silver 0:df754b773321 20
K2Silver 0:df754b773321 21 /* Speaker */
K2Silver 0:df754b773321 22 PwmOut speaker(p24);
K2Silver 0:df754b773321 23
K2Silver 0:df754b773321 24 /* Servo with IR sensor mounted */
K2Silver 0:df754b773321 25 Servo servo_ir(p23);
K2Silver 0:df754b773321 26
K2Silver 0:df754b773321 27 /* IR sensor */
K2Silver 0:df754b773321 28 AnalogIn ir_sensor(p20);
K2Silver 0:df754b773321 29
K2Silver 0:df754b773321 30 /* mbed LEDs */
K2Silver 0:df754b773321 31 DigitalOut led1(LED1);
K2Silver 0:df754b773321 32 DigitalOut led4(LED4);
K2Silver 0:df754b773321 33
K2Silver 0:df754b773321 34 /* IR sensor thread */
K2Silver 0:df754b773321 35 void ir_thread(void const * args) {
K2Silver 0:df754b773321 36 while (1) {
K2Silver 0:df754b773321 37 if (ir_sensor > 0.8) {
K2Silver 0:df754b773321 38 speaker = 1;
K2Silver 0:df754b773321 39 } else {
K2Silver 0:df754b773321 40 speaker = 0;
K2Silver 0:df754b773321 41 }
K2Silver 0:df754b773321 42 Thread::wait(50);
K2Silver 0:df754b773321 43 }
K2Silver 0:df754b773321 44 }
K2Silver 0:df754b773321 45
K2Silver 0:df754b773321 46 /* Circular buffer to hold commands */
K2Silver 0:df754b773321 47 static int COM_BUF_SIZE = 100;
K2Silver 0:df754b773321 48 volatile char * command_queue;
K2Silver 0:df754b773321 49 volatile int queue_tail = 0;
K2Silver 0:df754b773321 50 volatile int queue_head = 0;
K2Silver 0:df754b773321 51
K2Silver 0:df754b773321 52 /* Command execute thread */
K2Silver 0:df754b773321 53 void command_exec_thread(void const *args) {
K2Silver 0:df754b773321 54 while (1) {
K2Silver 0:df754b773321 55 if (queue_tail < queue_head) {
K2Silver 0:df754b773321 56 /* Save comand in temporary char variable */
K2Silver 0:df754b773321 57 char command = command_queue[queue_tail];
K2Silver 0:df754b773321 58
K2Silver 0:df754b773321 59 /* Null the command consumed */
K2Silver 0:df754b773321 60 command_queue[queue_tail] = '\0';
K2Silver 0:df754b773321 61
K2Silver 0:df754b773321 62 /* Execute the command */
K2Silver 0:df754b773321 63 switch(command) {
K2Silver 0:df754b773321 64 case '\0': /* null command */
K2Silver 0:df754b773321 65 break;
K2Silver 0:df754b773321 66 case 'F': /* Move forward */
K2Silver 0:df754b773321 67 motor_r.speed(1);
K2Silver 0:df754b773321 68 motor_l.speed(1);
K2Silver 0:df754b773321 69 Thread::wait(400);
K2Silver 0:df754b773321 70 motor_r.stop(1);
K2Silver 0:df754b773321 71 motor_l.stop(1);
K2Silver 0:df754b773321 72 break;
K2Silver 0:df754b773321 73 case 'B': /* Move backwards */
K2Silver 0:df754b773321 74 motor_r.speed(-1);
K2Silver 0:df754b773321 75 motor_l.speed(-1);
K2Silver 0:df754b773321 76 Thread::wait(400);
K2Silver 0:df754b773321 77 motor_r.stop(1);
K2Silver 0:df754b773321 78 motor_l.stop(1);
K2Silver 0:df754b773321 79 break;
K2Silver 0:df754b773321 80 case 'R': /* Move right */
K2Silver 0:df754b773321 81 motor_l.speed(1);
K2Silver 0:df754b773321 82 Thread::wait(400);
K2Silver 0:df754b773321 83 motor_l.stop(1);
K2Silver 0:df754b773321 84 break;
K2Silver 0:df754b773321 85 case 'L': /* Move left */
K2Silver 0:df754b773321 86 motor_r.speed(1);
K2Silver 0:df754b773321 87 Thread::wait(400);
K2Silver 0:df754b773321 88 motor_r.stop(1);
K2Silver 0:df754b773321 89 break;
K2Silver 0:df754b773321 90 case 'S': /* Turn servo right */
K2Silver 0:df754b773321 91 if (servo_ir > 0.0) {
K2Silver 0:df754b773321 92 float val = (servo_ir - .2);
K2Silver 0:df754b773321 93 servo_ir = val > 0.0 ? val : 0.0;
K2Silver 0:df754b773321 94 }
K2Silver 0:df754b773321 95 break;
K2Silver 0:df754b773321 96 case 'P': /* Turn servo left */
K2Silver 0:df754b773321 97 if (servo_ir < 1.0) {
K2Silver 0:df754b773321 98 float val = (servo_ir + .2);
K2Silver 0:df754b773321 99 servo_ir = val < 1.0 ? val : 1.0;
K2Silver 0:df754b773321 100 }
K2Silver 0:df754b773321 101 break;
K2Silver 0:df754b773321 102 default:
K2Silver 0:df754b773321 103 break;
K2Silver 0:df754b773321 104 }
K2Silver 0:df754b773321 105 /* Increment command index in circular buffer */
K2Silver 0:df754b773321 106 queue_tail = (queue_tail + 1) % COM_BUF_SIZE;
K2Silver 0:df754b773321 107 }
K2Silver 0:df754b773321 108 Thread::wait(200);
K2Silver 0:df754b773321 109 }
K2Silver 0:df754b773321 110 }
K2Silver 0:df754b773321 111
K2Silver 0:df754b773321 112
K2Silver 0:df754b773321 113 /* Queue command thread*/
K2Silver 0:df754b773321 114 void command_queue_thread(void const *args) {
K2Silver 0:df754b773321 115 while(1) {
K2Silver 0:df754b773321 116 if (rcv_tail < rcv_head) {
K2Silver 0:df754b773321 117 if (rcv[rcv_tail] == 'Z') {
K2Silver 0:df754b773321 118 /* Loop until valid value found */
K2Silver 0:df754b773321 119 while (rcv[(rcv_tail + 1) % RCV_BUF_SIZE] == '\0') {
K2Silver 0:df754b773321 120 Thread::wait(100);
K2Silver 0:df754b773321 121 }
K2Silver 0:df754b773321 122 char let = rcv[(rcv_tail + 1) % RCV_BUF_SIZE];
K2Silver 0:df754b773321 123 command_queue[queue_head] = let;
K2Silver 0:df754b773321 124 queue_head = (queue_head + 1) % COM_BUF_SIZE;
K2Silver 0:df754b773321 125 }
K2Silver 0:df754b773321 126 rcv[rcv_tail] = '\0'; /* Clear value */
K2Silver 0:df754b773321 127 rcv_tail = (rcv_tail + 1) % RCV_BUF_SIZE;
K2Silver 0:df754b773321 128 }
K2Silver 0:df754b773321 129 Thread::wait(10);
K2Silver 0:df754b773321 130 }
K2Silver 0:df754b773321 131 }
K2Silver 0:df754b773321 132
K2Silver 0:df754b773321 133 /* PC receive interrupt routine */
K2Silver 0:df754b773321 134 void pc_recv() {
K2Silver 0:df754b773321 135 led1 = 1;
K2Silver 0:df754b773321 136 while(pc.readable()) {
K2Silver 0:df754b773321 137 /* Print out to ESP8266 hardware */
K2Silver 0:df754b773321 138 esp.putc(pc.getc());
K2Silver 0:df754b773321 139 }
K2Silver 0:df754b773321 140 led1 = 0;
K2Silver 0:df754b773321 141 }
K2Silver 0:df754b773321 142
K2Silver 0:df754b773321 143 /* ESP83266 receive interrupt routine */
K2Silver 0:df754b773321 144 void dev_recv() {
K2Silver 0:df754b773321 145 led4 = 1;
K2Silver 0:df754b773321 146 while(esp.readable()) {
K2Silver 0:df754b773321 147 char let = esp.getc();
K2Silver 0:df754b773321 148 /* Echo to pc to see output */
K2Silver 0:df754b773321 149 pc.putc(let);
K2Silver 0:df754b773321 150 /* Put in receive buffer */
K2Silver 0:df754b773321 151 rcv[rcv_head] = let;
K2Silver 0:df754b773321 152 rcv_head = (rcv_head + 1) % RCV_BUF_SIZE;
K2Silver 0:df754b773321 153 }
K2Silver 0:df754b773321 154 led4 = 0;
K2Silver 0:df754b773321 155 }
K2Silver 0:df754b773321 156
K2Silver 0:df754b773321 157 /* Main code */
K2Silver 0:df754b773321 158 int main() {
K2Silver 0:df754b773321 159 /* Set pc serial baud rate */
K2Silver 0:df754b773321 160 pc.baud(9600);
K2Silver 0:df754b773321 161
K2Silver 0:df754b773321 162 /* Set up esp */
K2Silver 0:df754b773321 163 esp.reset();
K2Silver 0:df754b773321 164 esp.baud(9600);
K2Silver 0:df754b773321 165 esp.setup();
K2Silver 0:df754b773321 166
K2Silver 0:df754b773321 167 /* Initialize command buffer */
K2Silver 0:df754b773321 168 command_queue = (char*) calloc(sizeof(char), COM_BUF_SIZE);
K2Silver 0:df754b773321 169
K2Silver 0:df754b773321 170 /* Attach interrupts */
K2Silver 0:df754b773321 171 pc.attach(&pc_recv, Serial::RxIrq);
K2Silver 0:df754b773321 172 esp.attach(&dev_recv, Serial::RxIrq);
K2Silver 0:df754b773321 173
K2Silver 0:df754b773321 174 /* Print IP and MAC */
K2Silver 0:df754b773321 175 esp.getIP();
K2Silver 0:df754b773321 176 esp.getMAC();
K2Silver 0:df754b773321 177
K2Silver 0:df754b773321 178 /* Start threads */
K2Silver 0:df754b773321 179 Thread command_exec_thr(command_exec_thread);
K2Silver 0:df754b773321 180 Thread command_queue_thr(command_queue_thread);
K2Silver 0:df754b773321 181 Thread ir_thr(ir_thread);
K2Silver 0:df754b773321 182
K2Silver 0:df754b773321 183 while(1) {
K2Silver 0:df754b773321 184 Thread::wait(10000);
K2Silver 0:df754b773321 185 }
K2Silver 0:df754b773321 186 }