File content as of revision 1:dcd6c9be9e4b:

#include "mbed.h"
#include "Camera_LS_Y201.h"
#include "SDFileSystem.h"
#include "HMC6352.h"
 
#define DEBMSG      printf
#define NEWLINE()   printf("\r\n")
 
#define USE_SDCARD 1
 
#if USE_SDCARD
#define FILENAME    "/sd/IMG_%04d.jpg"
SDFileSystem fs(p5, p6, p7, p8, "sd");
#else
#define FILENAME    "/local/IMG_%04d.jpg"
LocalFileSystem fs("local");
#endif
Camera_LS_Y201 cam1(p13, p14);
 
typedef struct work {
    FILE *fp;
} work_t;
 
work_t work;
 
Serial device(p28, p27);  // tx, rx
AnalogIn sensor(p18);
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
Serial pc(USBTX, USBRX);
AnalogIn front(p20);
AnalogIn back(p19);
 
// Definitions of iRobot Create OpenInterface Command Numbers
// See the Create OpenInterface manual for a complete list
 
 
//                 Create Command              // Arguments
const char         Start = 128;
const char         SafeMode = 131;
const char         FullMode = 132;
const char         Drive = 137;                // 4:   [Vel. Hi] [Vel Low] [Rad. Hi] [Rad. Low]
const char         DriveDirect = 145;          // 4:   [Right Hi] [Right Low] [Left Hi] [Left Low]
const char         Demo = 136;                 // 2:    Run Demo x
const char         Sensors = 142;              // 1:    Sensor Packet ID
const char         CoverandDock = 143;         // 1:    Return to Charger
const char         SensorStream = 148;               // x+1: [# of packets requested] IDs of requested packets to stream
const char         QueryList = 149;            // x+1: [# of packets requested] IDs of requested packets to stream
const char         StreamPause = 150;          // 1:    0 = stop stream, 1 = start stream
const char         PlaySong = 141;
const char         Song = 140;
                /* iRobot Create Sensor IDs */
const char         BumpsandDrops = 7;
const char         Distance = 19;
const char         Angle = 20;
 
int speed_left =  200;
int speed_right = 200;
int imgNum = 0;
void start();
void forward();
void reverse();
void left();
void right();
void stop();
void playsong();
void charger();
void testDistance();
int frontDetect();
int backDetect();
void detectProcess();
void sendCommand();
void callback_func();
int capture();
void picture();
void moveAndPhoto(int dir);


// Demo to move around using basic commands
int main() {
// wait for Create to power up to accept serial commands
    wait(5);
// set baud rate for Create factory default
    device.baud(57600);
        pc.baud(9600);
// Start command mode and select sensor data to send back
    start();
    wait(.5);
// Camera initialization
    DEBMSG("Camera module");
    NEWLINE();
    DEBMSG("Resetting...");
    NEWLINE();
    wait(1);
    if (cam1.reset() == 0) {
        DEBMSG("Reset OK.");
        NEWLINE();
    } else {
        DEBMSG("Reset fail.");
        NEWLINE();
        error("Reset fail.");
    }

    wait(1);
// sonar detection 
    float frontFirst;
    float backFirst;
    float frontIncoming;
    float backIncoming;
    frontFirst  = frontDetect();
    backFirst  = backDetect();
    printf("front first: %f \n", frontFirst);
    printf("back first: %f \n", backFirst);

    while(1) {
        wait(0.001);
        frontIncoming = frontDetect();
        if (abs(frontIncoming - frontFirst) > 20) {
            if (frontIncoming < 200 && frontFirst < 200) {
                printf("1st: %f \n", frontFirst);
                printf("2nd: %f \n", frontIncoming);
                led3 = 0;
                led1 = 1;
                printf("first: %f \n", frontFirst);
                printf("incoming: %f \n", frontIncoming);
                moveAndPhoto(1);
                frontFirst = frontDetect();
                led1 = 0;
            }
            else {
                led3 = 1;
                frontFirst = frontIncoming;
            }
        } else {
            frontFirst = frontIncoming;
        }
        backIncoming = backDetect();
        if (abs(backIncoming - backFirst) > 20) {
            if (backIncoming < 200 && backFirst < 200) {
                led3 = 0;
                led1 = 1;
                printf("first: %f \n", backFirst);
                printf("incoming: %f \n", backIncoming);
                moveAndPhoto(0);
                backFirst = backDetect();
                backFirst = backDetect();
                led1 = 0;
            }
            else {
                led3 = 1;
                backFirst = backIncoming;
            }
        } else {
            backFirst = backIncoming;
        }
    }

    
    /*
    forward();
    wait(.5);
    stop();
    bool flag = true;
    */
//    testDistance();
//    wait(10);
//        playsong();
// Move around with motor commands
    //forward();
//    wait(.5);
//    stop();
//    wait(.1);
//    reverse();
//    wait(.5);
//    left();
//    wait(1);
//    stop();
//    wait(.1);
//    right();
//    wait(1);
//    stop();
//    wait(.5);
// Play a song
//    playsong();
//    wait(10);
// Search for battery charger IR beacon
//    charger();
}
 
 
// Start  - send start and safe mode, start streaming sensor data
void start() {
   // device.printf("%c%c", Start, SafeMode);
    device.putc(Start);
    device.putc(SafeMode);
    wait(.5);
  //  device.printf("%c%c%c", SensorStream, char(1), BumpsandDrops);
    device.putc(SensorStream);
    device.putc(1);
    device.putc(BumpsandDrops);
    wait(.5);
}
// Stop  - turn off drive motors
void stop() {
    device.printf("%c%c%c%c%c", DriveDirect, char(0),  char(0),  char(0),  char(0));
}
// Forward  - turn on drive motors
void forward() {
    device.printf("%c%c%c%c%c", DriveDirect, char((speed_right>>8)&0xFF),  char(speed_right&0xFF),  
    char((speed_left>>8)&0xFF),  char(speed_left&0xFF));
 
}
// Reverse - reverse drive motors
void reverse() {
    device.printf("%c%c%c%c%c", DriveDirect, char(((-speed_right)>>8)&0xFF),  char((-speed_right)&0xFF),  
    char(((-speed_left)>>8)&0xFF),  char((-speed_left)&0xFF));
 
}
// Left - drive motors set to rotate to left
void left() {
    device.printf("%c%c%c%c%c", DriveDirect, char((speed_right>>8)&0xFF),  char(speed_right&0xFF),  
    char(((-speed_left)>>8)&0xFF),  char((-speed_left)&0xFF));
}
// Right - drive motors set to rotate to right
void right() {
    device.printf("%c%c%c%c%c", DriveDirect, char(((-speed_right)>>8)&0xFF),  char((-speed_right)&0xFF),  
    char((speed_left>>8)&0xFF),  char(speed_left&0xFF));
 
}
// Charger - search and return to charger using IR beacons (if found)
void charger() {
    device.printf("%c%c", Demo, char(1));
}
// 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));
}

void testDistance() {
    int timer = 0;
    while(timer < 100000000) {
        if (sensor > 0.75) {
            led1 = 1;
            led2 = 1;
            led3 = 1;
            led4 = 1;
            forward();
        } else if(sensor > 0.50){
            led1 = 1;
            led2 = 1;
            led3 = 1;
            led4 = 0;
            left();
            wait(1);
            forward();
            wait(1);
            stop();
        } else if(sensor > 0.25){
            led1 = 1;
            led2 = 1;
            led3 = 0;
            led4 = 0;
            right();
            wait(1);
            forward();
            wait(1);
            stop();
        } else if(sensor > 0.1){
            led1 = 1;
            led2 = 0;
            led3 = 0;
            led4 = 0;
//            forward();
    stop();
        } else {
            led1 = 0;
            led2 = 0;
            led3 = 0;
            led4 = 0;
            stop();
        }
        wait(1);
        timer = timer + 1;
        timer = timer - 1;
    }
}

int frontDetect() {
    
    float sum = 0;
    front.read(); //first reading not reliable
    for (int i = 0; i < 5; i++) {
    wait(0.01);
    float temp = front.read() * 1000.0;
    sum += temp;
    }
    return sum/5;
}

int backDetect() {
    
    float sum = 0;
    back.read(); //first reading not reliable
    for (int i = 0; i < 5; i++) {
    wait(0.01);
    float temp = back.read() * 1000.0;
    sum += temp;
    }
    return sum/5;
}
/*
bool detectProcess() {
    float first;
    float incoming;
    float threshold;
    first  = detect();
    while(1) {
        wait(0.001);
        incoming = detect();
        if (abs(incoming - first) > 15) {
            led1 = 1;
            return true;
        } else {
            first = incoming;
        }
    }
}
*/

void sendCommand() {
    
}


/**
 * Callback function for readJpegFileContent.
 *
 * @param buf A pointer to a buffer.
 * @param siz A size of the buffer.
 */
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();
    }
}
 
/**
 * Capture.
 *
 * @param cam A pointer to a camera object.
 * @param filename The file name.
 *
 * @return Return 0 if it succeed.
 */
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;
}
 
/**
 * Entry point.
 */
 
 

 

void picture() {
    char fname[64];
    snprintf(fname, sizeof(fname) - 1, FILENAME, imgNum);
    int r = capture(&cam1, fname);
    if (r == 0) {
        DEBMSG("[%04d]:OK.", imgNum);
        NEWLINE();
    } else {
        DEBMSG("[%04d]:NG. (code=%d)", imgNum, r);
        NEWLINE();
        error("Failure.");
    }
    wait(3);
    imgNum++;
}

void moveAndPhoto(int dir) {
    // dir: 1 stands for counter-clockwise, 2 for clockwise
    led2 = 1;
    if (dir == 1) {
        left(); // rotate counter-clockwise
        wait(1.3);
        stop(); // stop
    } else if (dir == 0) { // moving clockwise
        right();
        wait(1.3);
        stop();
    }
    led2 = 0;
    picture(); // take picture
    
}