#include "mbed.h"
#include "SDHCFileSystem.h"
#include "wavfile.h"
#include "Camera_LS_Y201.h"
Serial pc(USBTX, USBRX);
Serial esp(p28, p27); // tx, rx
Serial device(p9, p10); // tx, rx
Camera_LS_Y201 cam1(p13, p14);
//Cam and Mic code
#define DEBMSG printf
#define NEWLINE() printf("\r\n")
#define FILENAME "/sd/IMG_spy_%01d.jpg"
//#define FILENAME "/local/IMG_spy%001d.jpg"
#define RECORDNAME "/sd/rec_%001d.wav"
SDFileSystem fs(p5, p6, p7, p8, "sd");
//LocalFileSystem fs("local");
#define RAM_TOTAL 0x1000
AnalogOut dacout(p18);
AnalogIn adcin(p20);
//DigitalOut led_play_ok(LED1);
//DigitalOut led_rec_ok(LED2);
Ticker ticker;
//SDFileSystem sdc(p5, p6, p7, p8, "sdc");
// Definitions of iRobot Roomba SCI Command Numbers
// See the Roomba SCI manual for a complete list
// Create Command // Arguments
const char Start = 128;
const char Control = 130;
const char Drive = 137; // 4: [Vel. Hi] [Vel Low] [Rad. Hi] [Rad. Low]
const char Sensors = 142; // 1: Sensor Packet ID
const char CoverandDock = 143; // 0: Return to Charger
const char Clean = 135; // 0: Start Cleaning
const char PlaySong = 141;
const char Song = 140;
/* iRobot Roomba Sensor IDs */
const char BumpsandDrops = 1;
int speed = 400;
int radius = 0x8000;
void start();
void forward();
void reverse();
void left();
void right();
void stop();
void playsong();
void charger();
// Standard Mbed LED definitions
DigitalOut led1(LED1),led2(LED2),led3(LED3),led4(LED4);
//-------------------Cam and Mic functions------------------
float buffer[RAM_TOTAL];
int cnt = 0;
int rp = 0;
int wp = 0;
int dropout = 0;
#define WAVFILE_ERROR_PRINT(RESULT) \
do { \
WavFileResult R = RESULT; \
if (R != WavFileResultOK) { \
char wavfile_error_print_text[BUFSIZ]; \
wavfile_result_string(R, wavfile_error_print_text, sizeof(wavfile_error_print_text)); \
pc.printf("%s (code=%d)\r\n", wavfile_error_print_text, R); \
return 1; \
} \
} while(0)
//////
typedef struct work {
FILE *fp;
} work_t;
work_t work;
void callback_func(int done, int total, uint8_t *buf, size_t siz)
{
fwrite(buf, siz, 1, work.fp);
static int n = 0;
int tmp = done * 100 / total;
if (n != tmp) {
n = tmp;
DEBMSG("Writing...: %3d%%", n);
NEWLINE();
}
}
int capture(Camera_LS_Y201 *cam, char *filename)
{
/*
* Take a picture.
*/
if (cam->takePicture() != 0) {
return -1;
}
DEBMSG("Captured.");
NEWLINE();
/*
* Open file.
*/
work.fp = fopen(filename, "wb");
if (work.fp == NULL) {
return -2;
}
/*
* Read the content.
*/
DEBMSG("%s", filename);
NEWLINE();
if (cam->readJpegFileContent(callback_func) != 0) {
fclose(work.fp);
return -3;
}
fclose(work.fp);
/*
* Stop taking pictures.
*/
cam->stopTakingPictures();
return 0;
}
// things for sending/receiving data over serial
volatile int tx_in=0;
volatile int tx_out=0;
volatile int rx_in=0;
volatile int rx_out=0;
const int buffer_size = 2048;//4095;
char tx_buffer[64], rx_buffer[buffer_size],webdata[buffer_size];
void Tx_interrupt(),Rx_interrupt(),send_line(),read_line();
volatile int roombaDirection =0;
int DataRX, update, count, timeout;
char replybuff[buffer_size], cmdbuff[64], rx_line[buffer_size];
void SendCMD(),getreply(),ReadWebData(),moveRobo(),startserver();
void tickrec(void)
{
/*
* Check the rec overflow
*/
int np = (wp + 1) & (RAM_TOTAL - 1);
if (np != rp) {
buffer[wp] = adcin;
wp = np;
} else {
dropout++;
}
led2 = !led2;
//led_rec_ok = !led_rec_ok;
}
int rec(const char *filename, const int nsec)
{
led3=1;
WavFileResult result;
wavfile_info_t info;
wavfile_data_t data;
WAVFILE_INFO_AUDIO_FORMAT(&info) = 1;
WAVFILE_INFO_NUM_CHANNELS(&info) = 1;
WAVFILE_INFO_SAMPLE_RATE(&info) = 11025;
WAVFILE_INFO_BYTE_RATE(&info) = 22050;
WAVFILE_INFO_BLOCK_ALIGN(&info) = 2;
WAVFILE_INFO_BITS_PER_SAMPLE(&info) = 16;
WAVFILE *wf = wavfile_open(filename, WavFileModeWrite, &result);
WAVFILE_ERROR_PRINT(result);
WAVFILE_ERROR_PRINT(wavfile_write_info(wf, &info));
printf("[REC:%s]\r\n", filename);
printf("\tWAVFILE_INFO_AUDIO_FORMAT(&info) = %d\r\n", WAVFILE_INFO_AUDIO_FORMAT(&info));
printf("\tWAVFILE_INFO_NUM_CHANNELS(&info) = %d\r\n", WAVFILE_INFO_NUM_CHANNELS(&info));
printf("\tWAVFILE_INFO_SAMPLE_RATE(&info) = %d\r\n", WAVFILE_INFO_SAMPLE_RATE(&info));
printf("\tWAVFILE_INFO_BYTE_RATE(&info) = %d\r\n", WAVFILE_INFO_BYTE_RATE(&info));
printf("\tWAVFILE_INFO_BLOCK_ALIGN(&info) = %d\r\n", WAVFILE_INFO_BLOCK_ALIGN(&info));
printf("\tWAVFILE_INFO_BITS_PER_SAMPLE(&info) = %d\r\n", WAVFILE_INFO_BITS_PER_SAMPLE(&info));
const int interval_us = 1000000 / WAVFILE_INFO_SAMPLE_RATE(&info);
const unsigned int samples_for_nsec = WAVFILE_INFO_SAMPLE_RATE(&info) * nsec;
rp = 0;
wp = 0;
dropout = 0;
unsigned int count = 0;
ticker.attach_us(tickrec, interval_us);
WAVFILE_DATA_NUM_CHANNELS(&data) = 1;
while (1) {
while (rp == wp) {
wait_us(1);
}
WAVFILE_DATA_CHANNEL_DATA(&data, 0) = buffer[rp];
rp = (rp + 1) & (RAM_TOTAL - 1);
WAVFILE_ERROR_PRINT(wavfile_write_data(wf, &data));
count++;
if (count > samples_for_nsec) {
break;
}
}
ticker.detach();
//led_rec_ok = 0;
led3 = 0;
pc.printf("\t-- Rec done. (dropout=%d) --\r\n", dropout);
WAVFILE_ERROR_PRINT(wavfile_close(wf));
return 0;
}
//-------------------------Cam and Mic ends here-----------------
int main()
{
pc.baud(9600);
esp.baud(9600);
device.baud(57600);
led1=0,led2=0,led3=0, led4=0;
pc.printf("Welcome!\r\n");
start();
pc.printf("Playing sound!\r\n");
playsong();
for(int i=0;i<8;i++)
{
led1=!led1;wait(0.2);led1=!led1;led2=!led2;
wait(0.2);led2=!led2;led3=!led3;wait(0.2);
led3=!led3;led4=!led4;wait(0.2);led4=!led4;
}
led1=0,led2=0,led3=0, led4=0;
timeout=2;
esp.attach(&Rx_interrupt, Serial::RxIrq); // Setup a serial interrupt function to receive data
esp.attach(&Tx_interrupt, Serial::TxIrq); // Setup a serial interrupt function to transmit data
startserver();
DataRX=0;
count=0;
while(1) {
if(DataRX==1) {
pc.printf("\r\nReading Webdata!\r\n");
ReadWebData();
esp.attach(&Rx_interrupt, Serial::RxIrq);
}
if(update==1) // update time, hit count, and analog levels in the HUZZAH chip
{
pc.printf("Updating robot position!\r\n");
moveRobo();
count++;
update=0;
}
}
}
// Starts webserver
void startserver()
{
pc.printf("\n++++++++++ Setting up HTTP Server ++++++++++\r\n> ");
strcpy(cmdbuff, "ip, nm, gw=wifi.sta.getip()\r\n");
SendCMD();
getreply();
wait(0.2);
strcpy(cmdbuff,"print(\"IP Address: \",ip)\r\n");
SendCMD();
getreply();
wait(0.2);
//create server
sprintf(cmdbuff, "srv=net.createServer(net.TCP)\r\n");
SendCMD();
getreply();
wait(0.5);
strcpy(cmdbuff,"srv:listen(80,function(conn)\r\n");
SendCMD();
getreply();
wait(0.3);
strcpy(cmdbuff,"conn:on(\"receive\",function(conn,payload) \r\n");
SendCMD();
getreply();
wait(0.3);
//print data to mbed
strcpy(cmdbuff,"print(payload)\r\n");
SendCMD();
getreply();
wait(0.2);
//web page data
strcpy(cmdbuff,"conn:send('
IoT Controlled Spy Robot
')\r\n");
SendCMD();
getreply();
wait(0.4);
strcpy(cmdbuff,"conn:send('')\r\n");
SendCMD();
getreply();
wait(0.3);
//strcpy(cmdbuff,"conn:send('Camera Image
')\r\n");
//SendCMD();
//getreply();
//wait(0.3);
//strcpy(cmdbuff,"conn:send('')\r\n");
//SendCMD();
//getreply();
//wait(0.3);
strcpy(cmdbuff, "conn:send('Tip:
- Select a checkbox to specify direction
- Click SEND to send data to robot
')\r\n");
SendCMD();
getreply();
wait(0.5);
// end web page data
strcpy(cmdbuff, "conn:on(\"sent\",function(conn) conn:close() end)\r\n"); // close current connection
SendCMD();
getreply();
wait(0.3);
strcpy(cmdbuff, "end)\r\n");
SendCMD();
getreply();
wait(0.2);
strcpy(cmdbuff, "end)\r\n");
SendCMD();
getreply();
wait(0.2);
pc.printf("\n\n++++++++++ Ready ++++++++++\r\n\n");
}
// Reads and processes GET and POST web data
void ReadWebData()
{
wait_ms(200);
esp.attach(NULL,Serial::RxIrq);
DataRX=0;
memset(webdata, '\0', sizeof(webdata));
strcpy(webdata, rx_buffer);
memset(rx_buffer, '\0', sizeof(rx_buffer));
rx_in = 0;
rx_out = 0;
// check web data for form information
if( strstr(webdata, "check=led1v") != NULL ) {
//led1=!led1;
roombaDirection = 1;
}
if( strstr(webdata, "check=led2v") != NULL ) {
//led2=!led2;
roombaDirection =2;
}
if( strstr(webdata, "check=led3v") != NULL ) {
//led3=!led3;
roombaDirection =3;
}
if( strstr(webdata, "check=led4v") != NULL ) {
//led4=!led4;
roombaDirection =4;
}
if( strstr(webdata, "check=led5v") != NULL ) {
//led4=!led4;
roombaDirection =5;
}
if((strstr(webdata, "POST")!= NULL ) || ( strstr(webdata, "GET") != NULL)) { // set update flag if POST request
update=1;
}
}
void moveRobo()
{
switch (roombaDirection)
{
case 1:
{
forward();
wait(3);
stop();
roombaDirection =0;
pc.printf("Forward\r\n");
for(int i=0;i<4;i++)
led1=!led1;wait(0.2);led1=!led1;
break;
}
case 2:
{
reverse();
wait(3);
stop();
roombaDirection =0;
pc.printf("Backward\r\n");
for(int i=0;i<4;i++)
led2=!led2;wait(0.2);led2=!led2;
break;
}
case 3:
{
left();
wait(0.5);
stop();
wait(0.1);
forward();
wait(2);
stop();
roombaDirection =0;
pc.printf("Left\r\n");
for(int i=0;i<4;i++)
led3=!led3;wait(0.2);led3=!led3;
break;
}
case 4:
{
right();
wait(0.5);
stop();
wait(0.1);
forward();
wait(2);
stop();
roombaDirection =0;
pc.printf("Right\r\n");
for(int i=0;i<4;i++)
led3=!led3;wait(0.2);led3=!led3;
break;
}
case 5:
{
cnt++;
//char recname[32];
//snprintf(recname, sizeof(recname) - 1, RECORDNAME, cnt);
//static const char *target_filename = "/fs/rec-test1.wav"; //recname; //"
//pc.printf(target_filename);
//int val =rec(target_filename, 10);
//pc.printf("\rrec value: %d", val);
//while (1)
//{
/*
* 30 seconds recording.
*/
//if (rec(target_filename, 10) != 0) {
// break;
//}
//WavFileResult result;
//wavfile_info_t info;
//wavfile_data_t data;
//break;
//}
////
DEBMSG("Camera module");
NEWLINE();
DEBMSG("Resetting...");
NEWLINE();
wait(1);
if (cam1.reset() == 0) {
DEBMSG("Reset OK.");
NEWLINE();
led1=1;led2=0;
} else {
DEBMSG("Reset fail.");
NEWLINE();
error("Reset fail.");
led1=0;led2=0;
}
wait(1);
char fname[64];
//led1=1;led2=0;
snprintf(fname, sizeof(fname) - 1, FILENAME, cnt);
int r = capture(&cam1, fname);
if (r == 0)
{
DEBMSG("[%04d]:OK.", cnt);
led1=1;led2=1;
NEWLINE();
}
else
{
DEBMSG("[%04d]:NG. (code=%d)", cnt, r);
NEWLINE();
error("Failure.");
led1=0;led2=0;
}
}
}
}
// ESP Command data send
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;
}
// Get Command and ESP status replies
void getreply()
{
read_line();
sscanf(rx_line,replybuff);
}
// Read a line from the large rx buffer from rx interrupt routine
void read_line() {
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;
}
// Interupt Routine to read in data from serial port
void Rx_interrupt() {
DataRX=1;
//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;
}
//led3=0;
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;
}
//--------------------Roomba section----------------------------------
// Start - send start and safe mode, start streaming sensor data
void start() {
// device.printf("%c%c", Start, SafeMode);
device.putc(Start);
//device.putc(FullMode);
wait(.1);
device.putc(Control);
wait(.5);
// device.printf("%c%c", SensorStream, char(1));
device.putc(Sensors);
device.putc(BumpsandDrops);
wait(.5);
}
// Stop - turn off drive motors
void stop() {
device.printf("%c%c%c%c%c", Drive, char(0), char(0), char(0), char(0));
}
// Forward - turn on drive motors
void forward() {
device.printf("%c%c%c%c%c", Drive, char((speed>>8)&0xFF), char(speed&0xFF),
char((radius>>8)&0xFF), char(radius&0xFF));
}
// Reverse - reverse drive motors
void reverse() {
device.printf("%c%c%c%c%c", Drive, char(((-speed)>>8)&0xFF), char((-speed)&0xFF),
char(((radius)>>8)&0xFF), char((radius)&0xFF));
}
// Left - drive motors set to rotate to left
void left() {
device.printf("%c%c%c%c%c", Drive, char((speed>>8)&0xFF), char(speed&0xFF),
char(((1)>>8)&0xFF), char((1)&0xFF));
}
// Right - drive motors set to rotate to right
void right() {
device.printf("%c%c%c%c%c", Drive, char(((speed)>>8)&0xFF), char((speed)&0xFF),
char((-1>>8)&0xFF), char(-1&0xFF));
}
// Charger - search and return to charger using IR beacons (if found)
void charger() {
device.printf("%c", Clean );
wait(.2);
device.printf("%c", CoverandDock );
}
// Play Song - define and play a song
void playsong() { // Send out notes & duration to define song and then play song
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",
Song, char(0), char(16), char(91), char(24), char(89), char(12), char(87), char(36), char(87),
char(24), char(89), char(12), char(91), char(24), char(91), char(12), char(91), char(12), char(89),
char(12),char(87), char(12), char(89), char(12), char(91), char(12), char(89), char(12), char(87),
char(24), char(86), char(12), char(87), char(48));
wait(.2);
device.printf("%c%c", PlaySong, char(0));
}