Kehinde Aina / Mbed 2 deprecated Spy_Robot_Complete

ECE 4180 Final project: Kehinde, Marcus, Chidi, Sulemana

Dependencies:   Camera_LS_Y201 SDHCFileSystem mbed wavfile

Fork of HUZZAHESP8266-web-control-LPC1768 by Austin Dong

main.cpp@7:9c09d65e8245, 2016-12-11 (annotated)

Committer:
kennyainny
Date:
Sun Dec 11 20:36:53 2016 +0000
Revision:
7:9c09d65e8245
Parent:
6:9895ffe33a9f 
ECE 4180 source code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
star297 0:e2a155f50119 1 #include "mbed.h"
kennyainny 6:9895ffe33a9f 2 #include "SDHCFileSystem.h"
kennyainny 6:9895ffe33a9f 3 #include "wavfile.h"
kennyainny 6:9895ffe33a9f 4 #include "Camera_LS_Y201.h"
star297 0:e2a155f50119 5
star297 0:e2a155f50119 6 Serial pc(USBTX, USBRX);
4180_1 4:40dd020463ea 7 Serial esp(p28, p27); // tx, rx
kennyainny 6:9895ffe33a9f 8 Serial device(p9, p10); // tx, rx
kennyainny 6:9895ffe33a9f 9 Camera_LS_Y201 cam1(p13, p14);
star297 0:e2a155f50119 10
kennyainny 6:9895ffe33a9f 11 //Cam and Mic code
kennyainny 6:9895ffe33a9f 12 #define DEBMSG printf
kennyainny 6:9895ffe33a9f 13 #define NEWLINE() printf("\r\n")
kennyainny 6:9895ffe33a9f 14
kennyainny 6:9895ffe33a9f 15 #define FILENAME "/sd/IMG_spy_%01d.jpg"
kennyainny 6:9895ffe33a9f 16 //#define FILENAME "/local/IMG_spy%001d.jpg"
kennyainny 6:9895ffe33a9f 17 #define RECORDNAME "/sd/rec_%001d.wav"
kennyainny 6:9895ffe33a9f 18
kennyainny 6:9895ffe33a9f 19 SDFileSystem fs(p5, p6, p7, p8, "sd");
kennyainny 6:9895ffe33a9f 20
kennyainny 6:9895ffe33a9f 21 //LocalFileSystem fs("local");
kennyainny 6:9895ffe33a9f 22
kennyainny 6:9895ffe33a9f 23 #define RAM_TOTAL 0x1000
kennyainny 6:9895ffe33a9f 24
kennyainny 6:9895ffe33a9f 25 AnalogOut dacout(p18);
kennyainny 6:9895ffe33a9f 26 AnalogIn adcin(p20);
kennyainny 6:9895ffe33a9f 27 //DigitalOut led_play_ok(LED1);
kennyainny 6:9895ffe33a9f 28 //DigitalOut led_rec_ok(LED2);
kennyainny 6:9895ffe33a9f 29
kennyainny 6:9895ffe33a9f 30 Ticker ticker;
kennyainny 6:9895ffe33a9f 31 //SDFileSystem sdc(p5, p6, p7, p8, "sdc");
kennyainny 6:9895ffe33a9f 32
kennyainny 6:9895ffe33a9f 33 // Definitions of iRobot Roomba SCI Command Numbers
kennyainny 6:9895ffe33a9f 34 // See the Roomba SCI manual for a complete list
kennyainny 6:9895ffe33a9f 35 // Create Command // Arguments
kennyainny 6:9895ffe33a9f 36 const char Start = 128;
kennyainny 6:9895ffe33a9f 37 const char Control = 130;
kennyainny 6:9895ffe33a9f 38 const char Drive = 137; // 4: [Vel. Hi] [Vel Low] [Rad. Hi] [Rad. Low]
kennyainny 6:9895ffe33a9f 39 const char Sensors = 142; // 1: Sensor Packet ID
kennyainny 6:9895ffe33a9f 40 const char CoverandDock = 143; // 0: Return to Charger
kennyainny 6:9895ffe33a9f 41 const char Clean = 135; // 0: Start Cleaning
kennyainny 6:9895ffe33a9f 42 const char PlaySong = 141;
kennyainny 6:9895ffe33a9f 43 const char Song = 140;
kennyainny 6:9895ffe33a9f 44 /* iRobot Roomba Sensor IDs */
kennyainny 6:9895ffe33a9f 45 const char BumpsandDrops = 1;
kennyainny 6:9895ffe33a9f 46
kennyainny 6:9895ffe33a9f 47 int speed = 400;
kennyainny 6:9895ffe33a9f 48 int radius = 0x8000;
kennyainny 6:9895ffe33a9f 49 void start();
kennyainny 6:9895ffe33a9f 50 void forward();
kennyainny 6:9895ffe33a9f 51 void reverse();
kennyainny 6:9895ffe33a9f 52 void left();
kennyainny 6:9895ffe33a9f 53 void right();
kennyainny 6:9895ffe33a9f 54 void stop();
kennyainny 6:9895ffe33a9f 55 void playsong();
kennyainny 6:9895ffe33a9f 56 void charger();
star297 2:d4c6bc0f2dc4 57
star297 0:e2a155f50119 58 // Standard Mbed LED definitions
kennyainny 6:9895ffe33a9f 59 DigitalOut led1(LED1),led2(LED2),led3(LED3),led4(LED4);
kennyainny 6:9895ffe33a9f 60
kennyainny 6:9895ffe33a9f 61
kennyainny 6:9895ffe33a9f 62 //-------------------Cam and Mic functions------------------
kennyainny 6:9895ffe33a9f 63 float buffer[RAM_TOTAL];
kennyainny 6:9895ffe33a9f 64 int cnt = 0;
kennyainny 6:9895ffe33a9f 65 int rp = 0;
kennyainny 6:9895ffe33a9f 66 int wp = 0;
kennyainny 6:9895ffe33a9f 67 int dropout = 0;
star297 0:e2a155f50119 68
kennyainny 6:9895ffe33a9f 69 #define WAVFILE_ERROR_PRINT(RESULT) \
kennyainny 6:9895ffe33a9f 70 do { \
kennyainny 6:9895ffe33a9f 71 WavFileResult R = RESULT; \
kennyainny 6:9895ffe33a9f 72 if (R != WavFileResultOK) { \
kennyainny 6:9895ffe33a9f 73 char wavfile_error_print_text[BUFSIZ]; \
kennyainny 6:9895ffe33a9f 74 wavfile_result_string(R, wavfile_error_print_text, sizeof(wavfile_error_print_text)); \
kennyainny 6:9895ffe33a9f 75 pc.printf("%s (code=%d)\r\n", wavfile_error_print_text, R); \
kennyainny 6:9895ffe33a9f 76 return 1; \
kennyainny 6:9895ffe33a9f 77 } \
kennyainny 6:9895ffe33a9f 78 } while(0)
kennyainny 6:9895ffe33a9f 79
kennyainny 6:9895ffe33a9f 80 //////
kennyainny 6:9895ffe33a9f 81 typedef struct work {
kennyainny 6:9895ffe33a9f 82 FILE *fp;
kennyainny 6:9895ffe33a9f 83 } work_t;
kennyainny 6:9895ffe33a9f 84
kennyainny 6:9895ffe33a9f 85 work_t work;
star297 0:e2a155f50119 86
kennyainny 6:9895ffe33a9f 87 void callback_func(int done, int total, uint8_t *buf, size_t siz)
kennyainny 6:9895ffe33a9f 88 {
kennyainny 6:9895ffe33a9f 89 fwrite(buf, siz, 1, work.fp);
kennyainny 6:9895ffe33a9f 90
kennyainny 6:9895ffe33a9f 91 static int n = 0;
kennyainny 6:9895ffe33a9f 92 int tmp = done * 100 / total;
kennyainny 6:9895ffe33a9f 93 if (n != tmp) {
kennyainny 6:9895ffe33a9f 94 n = tmp;
kennyainny 6:9895ffe33a9f 95 DEBMSG("Writing...: %3d%%", n);
kennyainny 6:9895ffe33a9f 96 NEWLINE();
kennyainny 6:9895ffe33a9f 97 }
kennyainny 6:9895ffe33a9f 98 }
kennyainny 6:9895ffe33a9f 99
kennyainny 6:9895ffe33a9f 100 int capture(Camera_LS_Y201 *cam, char *filename)
kennyainny 6:9895ffe33a9f 101 {
kennyainny 6:9895ffe33a9f 102 /*
kennyainny 6:9895ffe33a9f 103 * Take a picture.
kennyainny 6:9895ffe33a9f 104 */
kennyainny 6:9895ffe33a9f 105 if (cam->takePicture() != 0) {
kennyainny 6:9895ffe33a9f 106 return -1;
kennyainny 6:9895ffe33a9f 107 }
kennyainny 6:9895ffe33a9f 108 DEBMSG("Captured.");
kennyainny 6:9895ffe33a9f 109 NEWLINE();
kennyainny 6:9895ffe33a9f 110
kennyainny 6:9895ffe33a9f 111 /*
kennyainny 6:9895ffe33a9f 112 * Open file.
kennyainny 6:9895ffe33a9f 113 */
kennyainny 6:9895ffe33a9f 114 work.fp = fopen(filename, "wb");
kennyainny 6:9895ffe33a9f 115 if (work.fp == NULL) {
kennyainny 6:9895ffe33a9f 116 return -2;
kennyainny 6:9895ffe33a9f 117 }
kennyainny 6:9895ffe33a9f 118
kennyainny 6:9895ffe33a9f 119 /*
kennyainny 6:9895ffe33a9f 120 * Read the content.
kennyainny 6:9895ffe33a9f 121 */
kennyainny 6:9895ffe33a9f 122 DEBMSG("%s", filename);
kennyainny 6:9895ffe33a9f 123 NEWLINE();
kennyainny 6:9895ffe33a9f 124 if (cam->readJpegFileContent(callback_func) != 0) {
kennyainny 6:9895ffe33a9f 125 fclose(work.fp);
kennyainny 6:9895ffe33a9f 126 return -3;
kennyainny 6:9895ffe33a9f 127 }
kennyainny 6:9895ffe33a9f 128 fclose(work.fp);
kennyainny 6:9895ffe33a9f 129
kennyainny 6:9895ffe33a9f 130 /*
kennyainny 6:9895ffe33a9f 131 * Stop taking pictures.
kennyainny 6:9895ffe33a9f 132 */
kennyainny 6:9895ffe33a9f 133 cam->stopTakingPictures();
kennyainny 6:9895ffe33a9f 134
kennyainny 6:9895ffe33a9f 135 return 0;
kennyainny 6:9895ffe33a9f 136 }
ausdong 5:bc0296a5ad8a 137
ausdong 5:bc0296a5ad8a 138 // things for sending/receiving data over serial
ausdong 5:bc0296a5ad8a 139 volatile int tx_in=0;
ausdong 5:bc0296a5ad8a 140 volatile int tx_out=0;
ausdong 5:bc0296a5ad8a 141 volatile int rx_in=0;
ausdong 5:bc0296a5ad8a 142 volatile int rx_out=0;
kennyainny 6:9895ffe33a9f 143 const int buffer_size = 2048;//4095;
kennyainny 6:9895ffe33a9f 144 char tx_buffer[64], rx_buffer[buffer_size],webdata[buffer_size];
kennyainny 6:9895ffe33a9f 145 void Tx_interrupt(),Rx_interrupt(),send_line(),read_line();
kennyainny 6:9895ffe33a9f 146
kennyainny 6:9895ffe33a9f 147 volatile int roombaDirection =0;
kennyainny 6:9895ffe33a9f 148
kennyainny 6:9895ffe33a9f 149 int DataRX, update, count, timeout;
kennyainny 6:9895ffe33a9f 150 char replybuff[buffer_size], cmdbuff[64], rx_line[buffer_size];
kennyainny 6:9895ffe33a9f 151 void SendCMD(),getreply(),ReadWebData(),moveRobo(),startserver();
kennyainny 6:9895ffe33a9f 152
kennyainny 6:9895ffe33a9f 153 void tickrec(void)
kennyainny 6:9895ffe33a9f 154 {
kennyainny 6:9895ffe33a9f 155 /*
kennyainny 6:9895ffe33a9f 156 * Check the rec overflow
kennyainny 6:9895ffe33a9f 157 */
kennyainny 6:9895ffe33a9f 158 int np = (wp + 1) & (RAM_TOTAL - 1);
kennyainny 6:9895ffe33a9f 159 if (np != rp) {
kennyainny 6:9895ffe33a9f 160 buffer[wp] = adcin;
kennyainny 6:9895ffe33a9f 161 wp = np;
kennyainny 6:9895ffe33a9f 162 } else {
kennyainny 6:9895ffe33a9f 163 dropout++;
kennyainny 6:9895ffe33a9f 164 }
kennyainny 6:9895ffe33a9f 165 led2 = !led2;
kennyainny 6:9895ffe33a9f 166 //led_rec_ok = !led_rec_ok;
kennyainny 6:9895ffe33a9f 167 }
kennyainny 6:9895ffe33a9f 168
kennyainny 6:9895ffe33a9f 169 int rec(const char *filename, const int nsec)
kennyainny 6:9895ffe33a9f 170 {
kennyainny 6:9895ffe33a9f 171 led3=1;
kennyainny 6:9895ffe33a9f 172 WavFileResult result;
kennyainny 6:9895ffe33a9f 173 wavfile_info_t info;
kennyainny 6:9895ffe33a9f 174 wavfile_data_t data;
kennyainny 6:9895ffe33a9f 175
kennyainny 6:9895ffe33a9f 176 WAVFILE_INFO_AUDIO_FORMAT(&info) = 1;
kennyainny 6:9895ffe33a9f 177 WAVFILE_INFO_NUM_CHANNELS(&info) = 1;
kennyainny 6:9895ffe33a9f 178 WAVFILE_INFO_SAMPLE_RATE(&info) = 11025;
kennyainny 6:9895ffe33a9f 179 WAVFILE_INFO_BYTE_RATE(&info) = 22050;
kennyainny 6:9895ffe33a9f 180 WAVFILE_INFO_BLOCK_ALIGN(&info) = 2;
kennyainny 6:9895ffe33a9f 181 WAVFILE_INFO_BITS_PER_SAMPLE(&info) = 16;
ausdong 5:bc0296a5ad8a 182
kennyainny 6:9895ffe33a9f 183 WAVFILE *wf = wavfile_open(filename, WavFileModeWrite, &result);
kennyainny 6:9895ffe33a9f 184 WAVFILE_ERROR_PRINT(result);
kennyainny 6:9895ffe33a9f 185 WAVFILE_ERROR_PRINT(wavfile_write_info(wf, &info));
kennyainny 6:9895ffe33a9f 186
kennyainny 6:9895ffe33a9f 187 printf("[REC:%s]\r\n", filename);
kennyainny 6:9895ffe33a9f 188 printf("\tWAVFILE_INFO_AUDIO_FORMAT(&info) = %d\r\n", WAVFILE_INFO_AUDIO_FORMAT(&info));
kennyainny 6:9895ffe33a9f 189 printf("\tWAVFILE_INFO_NUM_CHANNELS(&info) = %d\r\n", WAVFILE_INFO_NUM_CHANNELS(&info));
kennyainny 6:9895ffe33a9f 190 printf("\tWAVFILE_INFO_SAMPLE_RATE(&info) = %d\r\n", WAVFILE_INFO_SAMPLE_RATE(&info));
kennyainny 6:9895ffe33a9f 191 printf("\tWAVFILE_INFO_BYTE_RATE(&info) = %d\r\n", WAVFILE_INFO_BYTE_RATE(&info));
kennyainny 6:9895ffe33a9f 192 printf("\tWAVFILE_INFO_BLOCK_ALIGN(&info) = %d\r\n", WAVFILE_INFO_BLOCK_ALIGN(&info));
kennyainny 6:9895ffe33a9f 193 printf("\tWAVFILE_INFO_BITS_PER_SAMPLE(&info) = %d\r\n", WAVFILE_INFO_BITS_PER_SAMPLE(&info));
kennyainny 6:9895ffe33a9f 194
kennyainny 6:9895ffe33a9f 195 const int interval_us = 1000000 / WAVFILE_INFO_SAMPLE_RATE(&info);
kennyainny 6:9895ffe33a9f 196 const unsigned int samples_for_nsec = WAVFILE_INFO_SAMPLE_RATE(&info) * nsec;
kennyainny 6:9895ffe33a9f 197
kennyainny 6:9895ffe33a9f 198 rp = 0;
kennyainny 6:9895ffe33a9f 199 wp = 0;
kennyainny 6:9895ffe33a9f 200 dropout = 0;
kennyainny 6:9895ffe33a9f 201 unsigned int count = 0;
kennyainny 6:9895ffe33a9f 202 ticker.attach_us(tickrec, interval_us);
kennyainny 6:9895ffe33a9f 203 WAVFILE_DATA_NUM_CHANNELS(&data) = 1;
kennyainny 6:9895ffe33a9f 204 while (1) {
kennyainny 6:9895ffe33a9f 205 while (rp == wp) {
kennyainny 6:9895ffe33a9f 206 wait_us(1);
kennyainny 6:9895ffe33a9f 207 }
kennyainny 6:9895ffe33a9f 208
kennyainny 6:9895ffe33a9f 209 WAVFILE_DATA_CHANNEL_DATA(&data, 0) = buffer[rp];
kennyainny 6:9895ffe33a9f 210 rp = (rp + 1) & (RAM_TOTAL - 1);
kennyainny 6:9895ffe33a9f 211 WAVFILE_ERROR_PRINT(wavfile_write_data(wf, &data));
kennyainny 6:9895ffe33a9f 212
kennyainny 6:9895ffe33a9f 213 count++;
kennyainny 6:9895ffe33a9f 214 if (count > samples_for_nsec) {
kennyainny 6:9895ffe33a9f 215 break;
kennyainny 6:9895ffe33a9f 216 }
kennyainny 6:9895ffe33a9f 217 }
kennyainny 6:9895ffe33a9f 218 ticker.detach();
kennyainny 6:9895ffe33a9f 219 //led_rec_ok = 0;
kennyainny 6:9895ffe33a9f 220 led3 = 0;
kennyainny 6:9895ffe33a9f 221 pc.printf("\t-- Rec done. (dropout=%d) --\r\n", dropout);
kennyainny 6:9895ffe33a9f 222
kennyainny 6:9895ffe33a9f 223 WAVFILE_ERROR_PRINT(wavfile_close(wf));
kennyainny 6:9895ffe33a9f 224 return 0;
kennyainny 6:9895ffe33a9f 225 }
kennyainny 6:9895ffe33a9f 226 //-------------------------Cam and Mic ends here-----------------
kennyainny 6:9895ffe33a9f 227
star297 2:d4c6bc0f2dc4 228
4180_1 4:40dd020463ea 229 int main()
kennyainny 6:9895ffe33a9f 230 {
ausdong 5:bc0296a5ad8a 231 pc.baud(9600);
ausdong 5:bc0296a5ad8a 232 esp.baud(9600);
kennyainny 6:9895ffe33a9f 233 device.baud(57600);
kennyainny 6:9895ffe33a9f 234
kennyainny 6:9895ffe33a9f 235 led1=0,led2=0,led3=0, led4=0;
kennyainny 6:9895ffe33a9f 236
kennyainny 6:9895ffe33a9f 237 pc.printf("Welcome!\r\n");
kennyainny 6:9895ffe33a9f 238
kennyainny 6:9895ffe33a9f 239 start();
kennyainny 6:9895ffe33a9f 240
kennyainny 6:9895ffe33a9f 241 pc.printf("Playing sound!\r\n");
kennyainny 6:9895ffe33a9f 242 playsong();
kennyainny 6:9895ffe33a9f 243
kennyainny 6:9895ffe33a9f 244 for(int i=0;i<8;i++)
kennyainny 6:9895ffe33a9f 245 {
kennyainny 6:9895ffe33a9f 246 led1=!led1;wait(0.2);led1=!led1;led2=!led2;
kennyainny 6:9895ffe33a9f 247 wait(0.2);led2=!led2;led3=!led3;wait(0.2);
kennyainny 6:9895ffe33a9f 248 led3=!led3;led4=!led4;wait(0.2);led4=!led4;
kennyainny 6:9895ffe33a9f 249 }
kennyainny 6:9895ffe33a9f 250 led1=0,led2=0,led3=0, led4=0;
kennyainny 6:9895ffe33a9f 251
kennyainny 6:9895ffe33a9f 252 timeout=2;
kennyainny 6:9895ffe33a9f 253
kennyainny 6:9895ffe33a9f 254 esp.attach(&Rx_interrupt, Serial::RxIrq); // Setup a serial interrupt function to receive data
kennyainny 6:9895ffe33a9f 255 esp.attach(&Tx_interrupt, Serial::TxIrq); // Setup a serial interrupt function to transmit data
kennyainny 6:9895ffe33a9f 256
star297 0:e2a155f50119 257 startserver();
kennyainny 6:9895ffe33a9f 258
ausdong 5:bc0296a5ad8a 259 DataRX=0;
ausdong 5:bc0296a5ad8a 260 count=0;
kennyainny 6:9895ffe33a9f 261
4180_1 4:40dd020463ea 262 while(1) {
4180_1 4:40dd020463ea 263 if(DataRX==1) {
kennyainny 6:9895ffe33a9f 264 pc.printf("\r\nReading Webdata!\r\n");
kennyainny 6:9895ffe33a9f 265
star297 0:e2a155f50119 266 ReadWebData();
ausdong 5:bc0296a5ad8a 267 esp.attach(&Rx_interrupt, Serial::RxIrq);
ausdong 5:bc0296a5ad8a 268 }
ausdong 5:bc0296a5ad8a 269 if(update==1) // update time, hit count, and analog levels in the HUZZAH chip
ausdong 5:bc0296a5ad8a 270 {
kennyainny 6:9895ffe33a9f 271 pc.printf("Updating robot position!\r\n");
kennyainny 6:9895ffe33a9f 272 moveRobo();
ausdong 5:bc0296a5ad8a 273 count++;
ausdong 5:bc0296a5ad8a 274 update=0;
4180_1 4:40dd020463ea 275 }
star297 0:e2a155f50119 276 }
4180_1 4:40dd020463ea 277 }
star297 0:e2a155f50119 278
ausdong 5:bc0296a5ad8a 279 // Starts webserver
star297 0:e2a155f50119 280 void startserver()
kennyainny 6:9895ffe33a9f 281 {
kennyainny 6:9895ffe33a9f 282 pc.printf("\n++++++++++ Setting up HTTP Server ++++++++++\r\n> ");
ausdong 5:bc0296a5ad8a 283
kennyainny 6:9895ffe33a9f 284 strcpy(cmdbuff, "ip, nm, gw=wifi.sta.getip()\r\n");
ausdong 5:bc0296a5ad8a 285 SendCMD();
ausdong 5:bc0296a5ad8a 286 getreply();
kennyainny 6:9895ffe33a9f 287 wait(0.2);
kennyainny 6:9895ffe33a9f 288 strcpy(cmdbuff,"print(\"IP Address: \",ip)\r\n");
kennyainny 6:9895ffe33a9f 289 SendCMD();
kennyainny 6:9895ffe33a9f 290 getreply();
kennyainny 6:9895ffe33a9f 291 wait(0.2);
kennyainny 6:9895ffe33a9f 292
ausdong 5:bc0296a5ad8a 293 //create server
kennyainny 6:9895ffe33a9f 294 sprintf(cmdbuff, "srv=net.createServer(net.TCP)\r\n");
ausdong 5:bc0296a5ad8a 295 SendCMD();
ausdong 5:bc0296a5ad8a 296 getreply();
ausdong 5:bc0296a5ad8a 297 wait(0.5);
ausdong 5:bc0296a5ad8a 298 strcpy(cmdbuff,"srv:listen(80,function(conn)\r\n");
star297 0:e2a155f50119 299 SendCMD();
star297 1:71ed1afbf344 300 getreply();
ausdong 5:bc0296a5ad8a 301 wait(0.3);
ausdong 5:bc0296a5ad8a 302 strcpy(cmdbuff,"conn:on(\"receive\",function(conn,payload) \r\n");
ausdong 5:bc0296a5ad8a 303 SendCMD();
ausdong 5:bc0296a5ad8a 304 getreply();
ausdong 5:bc0296a5ad8a 305 wait(0.3);
ausdong 5:bc0296a5ad8a 306
ausdong 5:bc0296a5ad8a 307 //print data to mbed
ausdong 5:bc0296a5ad8a 308 strcpy(cmdbuff,"print(payload)\r\n");
ausdong 5:bc0296a5ad8a 309 SendCMD();
ausdong 5:bc0296a5ad8a 310 getreply();
ausdong 5:bc0296a5ad8a 311 wait(0.2);
ausdong 5:bc0296a5ad8a 312
ausdong 5:bc0296a5ad8a 313 //web page data
kennyainny 6:9895ffe33a9f 314 strcpy(cmdbuff,"conn:send('<!DOCTYPE html><html><body><h1>IoT Controlled Spy Robot</h1>')\r\n");
ausdong 5:bc0296a5ad8a 315 SendCMD();
ausdong 5:bc0296a5ad8a 316 getreply();
ausdong 5:bc0296a5ad8a 317 wait(0.4);
ausdong 5:bc0296a5ad8a 318 strcpy(cmdbuff,"conn:send('<form method=\"POST\"')\r\n");
ausdong 5:bc0296a5ad8a 319 SendCMD();
ausdong 5:bc0296a5ad8a 320 getreply();
ausdong 5:bc0296a5ad8a 321 wait(0.3);
kennyainny 6:9895ffe33a9f 322 strcpy(cmdbuff, "conn:send('<p><input type=\"checkbox\" name=\"check\" value=\"led1v\"> Forward</p>')\r\n");
ausdong 5:bc0296a5ad8a 323 SendCMD();
ausdong 5:bc0296a5ad8a 324 getreply();
ausdong 5:bc0296a5ad8a 325 wait(0.3);
kennyainny 6:9895ffe33a9f 326 strcpy(cmdbuff, "conn:send('<p><input type=\"checkbox\" name=\"check\" value=\"led2v\"> Backward</p>')\r\n");
kennyainny 6:9895ffe33a9f 327 SendCMD();
kennyainny 6:9895ffe33a9f 328 getreply();
kennyainny 6:9895ffe33a9f 329 wait(0.3);
kennyainny 6:9895ffe33a9f 330 strcpy(cmdbuff, "conn:send('<p><input type=\"checkbox\" name=\"check\" value=\"led3v\"> Left</p>')\r\n");
kennyainny 6:9895ffe33a9f 331 SendCMD();
kennyainny 6:9895ffe33a9f 332 getreply();
kennyainny 6:9895ffe33a9f 333 wait(0.3);
kennyainny 6:9895ffe33a9f 334 strcpy(cmdbuff, "conn:send('<p><input type=\"checkbox\" name=\"check\" value=\"led4v\"> Right</p>')\r\n");
ausdong 5:bc0296a5ad8a 335 SendCMD();
ausdong 5:bc0296a5ad8a 336 getreply();
ausdong 5:bc0296a5ad8a 337 wait(0.3);
kennyainny 6:9895ffe33a9f 338 strcpy(cmdbuff, "conn:send('<p><input type=\"checkbox\" name=\"check\" value=\"led5v\"> Snap-Rec</p>')\r\n");
kennyainny 6:9895ffe33a9f 339 SendCMD();
kennyainny 6:9895ffe33a9f 340 getreply();
kennyainny 6:9895ffe33a9f 341 wait(0.3);
kennyainny 6:9895ffe33a9f 342 strcpy(cmdbuff,"conn:send('<p><input type=\"submit\" value=\"SEND\"></p></form>')\r\n");
star297 3:f7febfa77784 343 SendCMD();
star297 2:d4c6bc0f2dc4 344 getreply();
ausdong 5:bc0296a5ad8a 345 wait(0.3);
kennyainny 6:9895ffe33a9f 346
kennyainny 6:9895ffe33a9f 347 //strcpy(cmdbuff,"conn:send('<h2>Camera Image</h2>')\r\n");
kennyainny 6:9895ffe33a9f 348 //SendCMD();
kennyainny 6:9895ffe33a9f 349 //getreply();
kennyainny 6:9895ffe33a9f 350 //wait(0.3);
kennyainny 6:9895ffe33a9f 351 //strcpy(cmdbuff,"conn:send('<img src=\"IMG_0001.jpg\" alt=\"cam-img\" style=\"width:320px;height:240px;\">')\r\n");
kennyainny 6:9895ffe33a9f 352 //SendCMD();
kennyainny 6:9895ffe33a9f 353 //getreply();
kennyainny 6:9895ffe33a9f 354 //wait(0.3);
kennyainny 6:9895ffe33a9f 355
kennyainny 6:9895ffe33a9f 356 strcpy(cmdbuff, "conn:send('<p><h2>Tip: </h2></p><ul><li>Select a checkbox to specify direction</li><li>Click SEND to send data to robot</li></ul></body></html>')\r\n");
ausdong 5:bc0296a5ad8a 357 SendCMD();
ausdong 5:bc0296a5ad8a 358 getreply();
ausdong 5:bc0296a5ad8a 359 wait(0.5);
ausdong 5:bc0296a5ad8a 360 // end web page data
ausdong 5:bc0296a5ad8a 361 strcpy(cmdbuff, "conn:on(\"sent\",function(conn) conn:close() end)\r\n"); // close current connection
ausdong 5:bc0296a5ad8a 362 SendCMD();
ausdong 5:bc0296a5ad8a 363 getreply();
ausdong 5:bc0296a5ad8a 364 wait(0.3);
ausdong 5:bc0296a5ad8a 365 strcpy(cmdbuff, "end)\r\n");
ausdong 5:bc0296a5ad8a 366 SendCMD();
ausdong 5:bc0296a5ad8a 367 getreply();
ausdong 5:bc0296a5ad8a 368 wait(0.2);
ausdong 5:bc0296a5ad8a 369 strcpy(cmdbuff, "end)\r\n");
ausdong 5:bc0296a5ad8a 370 SendCMD();
ausdong 5:bc0296a5ad8a 371 getreply();
ausdong 5:bc0296a5ad8a 372 wait(0.2);
ausdong 5:bc0296a5ad8a 373
ausdong 5:bc0296a5ad8a 374 pc.printf("\n\n++++++++++ Ready ++++++++++\r\n\n");
4180_1 4:40dd020463ea 375 }
ausdong 5:bc0296a5ad8a 376
kennyainny 6:9895ffe33a9f 377 // Reads and processes GET and POST web data
kennyainny 6:9895ffe33a9f 378 void ReadWebData()
kennyainny 6:9895ffe33a9f 379 {
kennyainny 6:9895ffe33a9f 380 wait_ms(200);
kennyainny 6:9895ffe33a9f 381 esp.attach(NULL,Serial::RxIrq);
kennyainny 6:9895ffe33a9f 382 DataRX=0;
kennyainny 6:9895ffe33a9f 383 memset(webdata, '\0', sizeof(webdata));
kennyainny 6:9895ffe33a9f 384 strcpy(webdata, rx_buffer);
kennyainny 6:9895ffe33a9f 385 memset(rx_buffer, '\0', sizeof(rx_buffer));
kennyainny 6:9895ffe33a9f 386 rx_in = 0;
kennyainny 6:9895ffe33a9f 387 rx_out = 0;
kennyainny 6:9895ffe33a9f 388 // check web data for form information
kennyainny 6:9895ffe33a9f 389 if( strstr(webdata, "check=led1v") != NULL ) {
kennyainny 6:9895ffe33a9f 390 //led1=!led1;
kennyainny 6:9895ffe33a9f 391 roombaDirection = 1;
kennyainny 6:9895ffe33a9f 392 }
kennyainny 6:9895ffe33a9f 393 if( strstr(webdata, "check=led2v") != NULL ) {
kennyainny 6:9895ffe33a9f 394 //led2=!led2;
kennyainny 6:9895ffe33a9f 395 roombaDirection =2;
kennyainny 6:9895ffe33a9f 396 }
kennyainny 6:9895ffe33a9f 397 if( strstr(webdata, "check=led3v") != NULL ) {
kennyainny 6:9895ffe33a9f 398 //led3=!led3;
kennyainny 6:9895ffe33a9f 399 roombaDirection =3;
kennyainny 6:9895ffe33a9f 400 }
kennyainny 6:9895ffe33a9f 401 if( strstr(webdata, "check=led4v") != NULL ) {
kennyainny 6:9895ffe33a9f 402 //led4=!led4;
kennyainny 6:9895ffe33a9f 403 roombaDirection =4;
kennyainny 6:9895ffe33a9f 404 }
kennyainny 6:9895ffe33a9f 405 if( strstr(webdata, "check=led5v") != NULL ) {
kennyainny 6:9895ffe33a9f 406 //led4=!led4;
kennyainny 6:9895ffe33a9f 407 roombaDirection =5;
kennyainny 6:9895ffe33a9f 408 }
kennyainny 6:9895ffe33a9f 409 if((strstr(webdata, "POST")!= NULL ) || ( strstr(webdata, "GET") != NULL)) { // set update flag if POST request
kennyainny 6:9895ffe33a9f 410 update=1;
kennyainny 6:9895ffe33a9f 411 }
kennyainny 6:9895ffe33a9f 412 }
kennyainny 6:9895ffe33a9f 413
kennyainny 6:9895ffe33a9f 414 void moveRobo()
kennyainny 6:9895ffe33a9f 415 {
kennyainny 6:9895ffe33a9f 416 switch (roombaDirection)
kennyainny 6:9895ffe33a9f 417 {
kennyainny 6:9895ffe33a9f 418 case 1:
kennyainny 6:9895ffe33a9f 419 {
kennyainny 6:9895ffe33a9f 420 forward();
kennyainny 6:9895ffe33a9f 421 wait(3);
kennyainny 6:9895ffe33a9f 422 stop();
kennyainny 6:9895ffe33a9f 423 roombaDirection =0;
kennyainny 6:9895ffe33a9f 424 pc.printf("Forward\r\n");
kennyainny 6:9895ffe33a9f 425 for(int i=0;i<4;i++)
kennyainny 6:9895ffe33a9f 426 led1=!led1;wait(0.2);led1=!led1;
kennyainny 6:9895ffe33a9f 427 break;
kennyainny 6:9895ffe33a9f 428 }
kennyainny 6:9895ffe33a9f 429 case 2:
kennyainny 6:9895ffe33a9f 430 {
kennyainny 6:9895ffe33a9f 431 reverse();
kennyainny 6:9895ffe33a9f 432 wait(3);
kennyainny 6:9895ffe33a9f 433 stop();
kennyainny 6:9895ffe33a9f 434 roombaDirection =0;
kennyainny 6:9895ffe33a9f 435 pc.printf("Backward\r\n");
kennyainny 6:9895ffe33a9f 436 for(int i=0;i<4;i++)
kennyainny 6:9895ffe33a9f 437 led2=!led2;wait(0.2);led2=!led2;
kennyainny 6:9895ffe33a9f 438 break;
kennyainny 6:9895ffe33a9f 439 }
kennyainny 6:9895ffe33a9f 440 case 3:
kennyainny 6:9895ffe33a9f 441 {
kennyainny 6:9895ffe33a9f 442 left();
kennyainny 6:9895ffe33a9f 443 wait(0.5);
kennyainny 6:9895ffe33a9f 444 stop();
kennyainny 6:9895ffe33a9f 445 wait(0.1);
kennyainny 6:9895ffe33a9f 446 forward();
kennyainny 6:9895ffe33a9f 447 wait(2);
kennyainny 6:9895ffe33a9f 448 stop();
kennyainny 6:9895ffe33a9f 449 roombaDirection =0;
kennyainny 6:9895ffe33a9f 450 pc.printf("Left\r\n");
kennyainny 6:9895ffe33a9f 451 for(int i=0;i<4;i++)
kennyainny 6:9895ffe33a9f 452 led3=!led3;wait(0.2);led3=!led3;
kennyainny 6:9895ffe33a9f 453 break;
kennyainny 6:9895ffe33a9f 454 }
kennyainny 6:9895ffe33a9f 455 case 4:
kennyainny 6:9895ffe33a9f 456 {
kennyainny 6:9895ffe33a9f 457 right();
kennyainny 6:9895ffe33a9f 458 wait(0.5);
kennyainny 6:9895ffe33a9f 459 stop();
kennyainny 6:9895ffe33a9f 460 wait(0.1);
kennyainny 6:9895ffe33a9f 461 forward();
kennyainny 6:9895ffe33a9f 462 wait(2);
kennyainny 6:9895ffe33a9f 463 stop();
kennyainny 6:9895ffe33a9f 464 roombaDirection =0;
kennyainny 6:9895ffe33a9f 465 pc.printf("Right\r\n");
kennyainny 6:9895ffe33a9f 466 for(int i=0;i<4;i++)
kennyainny 6:9895ffe33a9f 467 led3=!led3;wait(0.2);led3=!led3;
kennyainny 6:9895ffe33a9f 468 break;
kennyainny 6:9895ffe33a9f 469 }
kennyainny 6:9895ffe33a9f 470 case 5:
kennyainny 6:9895ffe33a9f 471 {
kennyainny 6:9895ffe33a9f 472 cnt++;
kennyainny 6:9895ffe33a9f 473
kennyainny 6:9895ffe33a9f 474 //char recname[32];
kennyainny 6:9895ffe33a9f 475
kennyainny 6:9895ffe33a9f 476 //snprintf(recname, sizeof(recname) - 1, RECORDNAME, cnt);
kennyainny 6:9895ffe33a9f 477
kennyainny 6:9895ffe33a9f 478 //static const char *target_filename = "/fs/rec-test1.wav"; //recname; //"
kennyainny 6:9895ffe33a9f 479 //pc.printf(target_filename);
kennyainny 6:9895ffe33a9f 480
kennyainny 6:9895ffe33a9f 481 //int val =rec(target_filename, 10);
kennyainny 6:9895ffe33a9f 482
kennyainny 6:9895ffe33a9f 483 //pc.printf("\rrec value: %d", val);
kennyainny 6:9895ffe33a9f 484
kennyainny 6:9895ffe33a9f 485 //while (1)
kennyainny 6:9895ffe33a9f 486 //{
kennyainny 6:9895ffe33a9f 487 /*
kennyainny 6:9895ffe33a9f 488 * 30 seconds recording.
kennyainny 6:9895ffe33a9f 489 */
kennyainny 6:9895ffe33a9f 490 //if (rec(target_filename, 10) != 0) {
kennyainny 6:9895ffe33a9f 491 // break;
kennyainny 6:9895ffe33a9f 492 //}
kennyainny 6:9895ffe33a9f 493
kennyainny 6:9895ffe33a9f 494 //WavFileResult result;
kennyainny 6:9895ffe33a9f 495 //wavfile_info_t info;
kennyainny 6:9895ffe33a9f 496 //wavfile_data_t data;
kennyainny 6:9895ffe33a9f 497 //break;
kennyainny 6:9895ffe33a9f 498 //}
kennyainny 6:9895ffe33a9f 499
kennyainny 6:9895ffe33a9f 500 ////
kennyainny 6:9895ffe33a9f 501 DEBMSG("Camera module");
kennyainny 6:9895ffe33a9f 502 NEWLINE();
kennyainny 6:9895ffe33a9f 503 DEBMSG("Resetting...");
kennyainny 6:9895ffe33a9f 504 NEWLINE();
kennyainny 6:9895ffe33a9f 505 wait(1);
kennyainny 6:9895ffe33a9f 506
kennyainny 6:9895ffe33a9f 507 if (cam1.reset() == 0) {
kennyainny 6:9895ffe33a9f 508 DEBMSG("Reset OK.");
kennyainny 6:9895ffe33a9f 509 NEWLINE();
kennyainny 6:9895ffe33a9f 510 led1=1;led2=0;
kennyainny 6:9895ffe33a9f 511 } else {
kennyainny 6:9895ffe33a9f 512 DEBMSG("Reset fail.");
kennyainny 6:9895ffe33a9f 513 NEWLINE();
kennyainny 6:9895ffe33a9f 514 error("Reset fail.");
kennyainny 6:9895ffe33a9f 515 led1=0;led2=0;
kennyainny 6:9895ffe33a9f 516 }
kennyainny 6:9895ffe33a9f 517 wait(1);
kennyainny 6:9895ffe33a9f 518
kennyainny 6:9895ffe33a9f 519 char fname[64];
kennyainny 6:9895ffe33a9f 520
kennyainny 6:9895ffe33a9f 521 //led1=1;led2=0;
kennyainny 6:9895ffe33a9f 522 snprintf(fname, sizeof(fname) - 1, FILENAME, cnt);
kennyainny 6:9895ffe33a9f 523 int r = capture(&cam1, fname);
kennyainny 6:9895ffe33a9f 524 if (r == 0)
kennyainny 6:9895ffe33a9f 525 {
kennyainny 6:9895ffe33a9f 526 DEBMSG("[%04d]:OK.", cnt);
kennyainny 6:9895ffe33a9f 527 led1=1;led2=1;
kennyainny 6:9895ffe33a9f 528 NEWLINE();
kennyainny 6:9895ffe33a9f 529 }
kennyainny 6:9895ffe33a9f 530 else
kennyainny 6:9895ffe33a9f 531 {
kennyainny 6:9895ffe33a9f 532 DEBMSG("[%04d]:NG. (code=%d)", cnt, r);
kennyainny 6:9895ffe33a9f 533 NEWLINE();
kennyainny 6:9895ffe33a9f 534 error("Failure.");
kennyainny 6:9895ffe33a9f 535 led1=0;led2=0;
kennyainny 6:9895ffe33a9f 536 }
kennyainny 6:9895ffe33a9f 537
kennyainny 6:9895ffe33a9f 538 }
kennyainny 6:9895ffe33a9f 539 }
kennyainny 6:9895ffe33a9f 540 }
ausdong 5:bc0296a5ad8a 541
star297 0:e2a155f50119 542 // ESP Command data send
star297 0:e2a155f50119 543 void SendCMD()
star297 0:e2a155f50119 544 {
ausdong 5:bc0296a5ad8a 545 int i;
ausdong 5:bc0296a5ad8a 546 char temp_char;
ausdong 5:bc0296a5ad8a 547 bool empty;
ausdong 5:bc0296a5ad8a 548 i = 0;
ausdong 5:bc0296a5ad8a 549 // Start Critical Section - don't interrupt while changing global buffer variables
ausdong 5:bc0296a5ad8a 550 NVIC_DisableIRQ(UART1_IRQn);
ausdong 5:bc0296a5ad8a 551 empty = (tx_in == tx_out);
ausdong 5:bc0296a5ad8a 552 while ((i==0) || (cmdbuff[i-1] != '\n')) {
ausdong 5:bc0296a5ad8a 553 // Wait if buffer full
ausdong 5:bc0296a5ad8a 554 if (((tx_in + 1) % buffer_size) == tx_out) {
ausdong 5:bc0296a5ad8a 555 // End Critical Section - need to let interrupt routine empty buffer by sending
ausdong 5:bc0296a5ad8a 556 NVIC_EnableIRQ(UART1_IRQn);
ausdong 5:bc0296a5ad8a 557 while (((tx_in + 1) % buffer_size) == tx_out) {
ausdong 5:bc0296a5ad8a 558 }
ausdong 5:bc0296a5ad8a 559 // Start Critical Section - don't interrupt while changing global buffer variables
ausdong 5:bc0296a5ad8a 560 NVIC_DisableIRQ(UART1_IRQn);
ausdong 5:bc0296a5ad8a 561 }
ausdong 5:bc0296a5ad8a 562 tx_buffer[tx_in] = cmdbuff[i];
ausdong 5:bc0296a5ad8a 563 i++;
ausdong 5:bc0296a5ad8a 564 tx_in = (tx_in + 1) % buffer_size;
ausdong 5:bc0296a5ad8a 565 }
ausdong 5:bc0296a5ad8a 566 if (esp.writeable() && (empty)) {
ausdong 5:bc0296a5ad8a 567 temp_char = tx_buffer[tx_out];
ausdong 5:bc0296a5ad8a 568 tx_out = (tx_out + 1) % buffer_size;
ausdong 5:bc0296a5ad8a 569 // Send first character to start tx interrupts, if stopped
ausdong 5:bc0296a5ad8a 570 esp.putc(temp_char);
ausdong 5:bc0296a5ad8a 571 }
ausdong 5:bc0296a5ad8a 572 // End Critical Section
ausdong 5:bc0296a5ad8a 573 NVIC_EnableIRQ(UART1_IRQn);
ausdong 5:bc0296a5ad8a 574 return;
4180_1 4:40dd020463ea 575 }
ausdong 5:bc0296a5ad8a 576
4180_1 4:40dd020463ea 577 // Get Command and ESP status replies
star297 0:e2a155f50119 578 void getreply()
4180_1 4:40dd020463ea 579 {
ausdong 5:bc0296a5ad8a 580 read_line();
ausdong 5:bc0296a5ad8a 581 sscanf(rx_line,replybuff);
ausdong 5:bc0296a5ad8a 582 }
ausdong 5:bc0296a5ad8a 583
ausdong 5:bc0296a5ad8a 584 // Read a line from the large rx buffer from rx interrupt routine
ausdong 5:bc0296a5ad8a 585 void read_line() {
ausdong 5:bc0296a5ad8a 586 int i;
ausdong 5:bc0296a5ad8a 587 i = 0;
ausdong 5:bc0296a5ad8a 588 // Start Critical Section - don't interrupt while changing global buffer variables
ausdong 5:bc0296a5ad8a 589 NVIC_DisableIRQ(UART1_IRQn);
ausdong 5:bc0296a5ad8a 590 // Loop reading rx buffer characters until end of line character
ausdong 5:bc0296a5ad8a 591 while ((i==0) || (rx_line[i-1] != '\r')) {
ausdong 5:bc0296a5ad8a 592 // Wait if buffer empty
ausdong 5:bc0296a5ad8a 593 if (rx_in == rx_out) {
ausdong 5:bc0296a5ad8a 594 // End Critical Section - need to allow rx interrupt to get new characters for buffer
ausdong 5:bc0296a5ad8a 595 NVIC_EnableIRQ(UART1_IRQn);
ausdong 5:bc0296a5ad8a 596 while (rx_in == rx_out) {
ausdong 5:bc0296a5ad8a 597 }
ausdong 5:bc0296a5ad8a 598 // Start Critical Section - don't interrupt while changing global buffer variables
ausdong 5:bc0296a5ad8a 599 NVIC_DisableIRQ(UART1_IRQn);
star297 2:d4c6bc0f2dc4 600 }
ausdong 5:bc0296a5ad8a 601 rx_line[i] = rx_buffer[rx_out];
ausdong 5:bc0296a5ad8a 602 i++;
ausdong 5:bc0296a5ad8a 603 rx_out = (rx_out + 1) % buffer_size;
4180_1 4:40dd020463ea 604 }
ausdong 5:bc0296a5ad8a 605 // End Critical Section
ausdong 5:bc0296a5ad8a 606 NVIC_EnableIRQ(UART1_IRQn);
ausdong 5:bc0296a5ad8a 607 rx_line[i-1] = 0;
ausdong 5:bc0296a5ad8a 608 return;
ausdong 5:bc0296a5ad8a 609 }
kennyainny 6:9895ffe33a9f 610
ausdong 5:bc0296a5ad8a 611 // Interupt Routine to read in data from serial port
ausdong 5:bc0296a5ad8a 612 void Rx_interrupt() {
ausdong 5:bc0296a5ad8a 613 DataRX=1;
ausdong 5:bc0296a5ad8a 614 //led3=1;
ausdong 5:bc0296a5ad8a 615 // Loop just in case more than one character is in UART's receive FIFO buffer
ausdong 5:bc0296a5ad8a 616 // Stop if buffer full
ausdong 5:bc0296a5ad8a 617 while ((esp.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
ausdong 5:bc0296a5ad8a 618 rx_buffer[rx_in] = esp.getc();
ausdong 5:bc0296a5ad8a 619 // Uncomment to Echo to USB serial to watch data flow
ausdong 5:bc0296a5ad8a 620 pc.putc(rx_buffer[rx_in]);
ausdong 5:bc0296a5ad8a 621 rx_in = (rx_in + 1) % buffer_size;
ausdong 5:bc0296a5ad8a 622 }
ausdong 5:bc0296a5ad8a 623 //led3=0;
ausdong 5:bc0296a5ad8a 624 return;
ausdong 5:bc0296a5ad8a 625 }
ausdong 5:bc0296a5ad8a 626
ausdong 5:bc0296a5ad8a 627 // Interupt Routine to write out data to serial port
ausdong 5:bc0296a5ad8a 628 void Tx_interrupt() {
ausdong 5:bc0296a5ad8a 629 //led2=1;
ausdong 5:bc0296a5ad8a 630 // Loop to fill more than one character in UART's transmit FIFO buffer
ausdong 5:bc0296a5ad8a 631 // Stop if buffer empty
ausdong 5:bc0296a5ad8a 632 while ((esp.writeable()) && (tx_in != tx_out)) {
ausdong 5:bc0296a5ad8a 633 esp.putc(tx_buffer[tx_out]);
ausdong 5:bc0296a5ad8a 634 tx_out = (tx_out + 1) % buffer_size;
ausdong 5:bc0296a5ad8a 635 }
ausdong 5:bc0296a5ad8a 636 //led2=0;
ausdong 5:bc0296a5ad8a 637 return;
kennyainny 6:9895ffe33a9f 638 }
kennyainny 6:9895ffe33a9f 639
ausdong 5:bc0296a5ad8a 640
kennyainny 6:9895ffe33a9f 641 //--------------------Roomba section----------------------------------
kennyainny 6:9895ffe33a9f 642
kennyainny 6:9895ffe33a9f 643 // Start - send start and safe mode, start streaming sensor data
kennyainny 6:9895ffe33a9f 644 void start() {
kennyainny 6:9895ffe33a9f 645 // device.printf("%c%c", Start, SafeMode);
kennyainny 6:9895ffe33a9f 646 device.putc(Start);
kennyainny 6:9895ffe33a9f 647
kennyainny 6:9895ffe33a9f 648 //device.putc(FullMode);
ausdong 5:bc0296a5ad8a 649
kennyainny 6:9895ffe33a9f 650 wait(.1);
kennyainny 6:9895ffe33a9f 651 device.putc(Control);
kennyainny 6:9895ffe33a9f 652 wait(.5);
kennyainny 6:9895ffe33a9f 653 // device.printf("%c%c", SensorStream, char(1));
kennyainny 6:9895ffe33a9f 654 device.putc(Sensors);
kennyainny 6:9895ffe33a9f 655 device.putc(BumpsandDrops);
kennyainny 6:9895ffe33a9f 656 wait(.5);
kennyainny 6:9895ffe33a9f 657 }
kennyainny 6:9895ffe33a9f 658 // Stop - turn off drive motors
kennyainny 6:9895ffe33a9f 659 void stop() {
kennyainny 6:9895ffe33a9f 660 device.printf("%c%c%c%c%c", Drive, char(0), char(0), char(0), char(0));
kennyainny 6:9895ffe33a9f 661 }
kennyainny 6:9895ffe33a9f 662 // Forward - turn on drive motors
kennyainny 6:9895ffe33a9f 663 void forward() {
kennyainny 6:9895ffe33a9f 664 device.printf("%c%c%c%c%c", Drive, char((speed>>8)&0xFF), char(speed&0xFF),
kennyainny 6:9895ffe33a9f 665 char((radius>>8)&0xFF), char(radius&0xFF));
kennyainny 6:9895ffe33a9f 666
kennyainny 6:9895ffe33a9f 667 }
kennyainny 6:9895ffe33a9f 668 // Reverse - reverse drive motors
kennyainny 6:9895ffe33a9f 669 void reverse() {
kennyainny 6:9895ffe33a9f 670 device.printf("%c%c%c%c%c", Drive, char(((-speed)>>8)&0xFF), char((-speed)&0xFF),
kennyainny 6:9895ffe33a9f 671 char(((radius)>>8)&0xFF), char((radius)&0xFF));
kennyainny 6:9895ffe33a9f 672
star297 0:e2a155f50119 673 }
kennyainny 6:9895ffe33a9f 674 // Left - drive motors set to rotate to left
kennyainny 6:9895ffe33a9f 675 void left() {
kennyainny 6:9895ffe33a9f 676 device.printf("%c%c%c%c%c", Drive, char((speed>>8)&0xFF), char(speed&0xFF),
kennyainny 6:9895ffe33a9f 677 char(((1)>>8)&0xFF), char((1)&0xFF));
kennyainny 6:9895ffe33a9f 678 }
kennyainny 6:9895ffe33a9f 679 // Right - drive motors set to rotate to right
kennyainny 6:9895ffe33a9f 680 void right() {
kennyainny 6:9895ffe33a9f 681 device.printf("%c%c%c%c%c", Drive, char(((speed)>>8)&0xFF), char((speed)&0xFF),
kennyainny 6:9895ffe33a9f 682 char((-1>>8)&0xFF), char(-1&0xFF));
kennyainny 6:9895ffe33a9f 683
star297 0:e2a155f50119 684 }
kennyainny 6:9895ffe33a9f 685 // Charger - search and return to charger using IR beacons (if found)
kennyainny 6:9895ffe33a9f 686 void charger() {
kennyainny 6:9895ffe33a9f 687 device.printf("%c", Clean );
kennyainny 6:9895ffe33a9f 688 wait(.2);
kennyainny 6:9895ffe33a9f 689 device.printf("%c", CoverandDock );
kennyainny 6:9895ffe33a9f 690 }
kennyainny 6:9895ffe33a9f 691 // Play Song - define and play a song
kennyainny 6:9895ffe33a9f 692 void playsong() { // Send out notes & duration to define song and then play song
4180_1 4:40dd020463ea 693
kennyainny 6:9895ffe33a9f 694 device.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",
kennyainny 6:9895ffe33a9f 695 Song, char(0), char(16), char(91), char(24), char(89), char(12), char(87), char(36), char(87),
kennyainny 6:9895ffe33a9f 696 char(24), char(89), char(12), char(91), char(24), char(91), char(12), char(91), char(12), char(89),
kennyainny 6:9895ffe33a9f 697 char(12),char(87), char(12), char(89), char(12), char(91), char(12), char(89), char(12), char(87),
kennyainny 6:9895ffe33a9f 698 char(24), char(86), char(12), char(87), char(48));
kennyainny 6:9895ffe33a9f 699
kennyainny 6:9895ffe33a9f 700 wait(.2);
kennyainny 6:9895ffe33a9f 701 device.printf("%c%c", PlaySong, char(0));
kennyainny 6:9895ffe33a9f 702 }
kennyainny 6:9895ffe33a9f 703