Diff: main.cpp

Revision:

1:dcd6c9be9e4b

Parent:

0:21c11d775cc4

--- a/main.cpp	Fri Apr 17 00:07:38 2015 +0000
+++ b/main.cpp	Thu Apr 30 19:01:22 2015 +0000
@@ -1,11 +1,37 @@
 #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
  
-Serial device(p9, p10);  // tx, rx
-AnalogIn sensor(p20);
+#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
@@ -32,6 +58,7 @@
  
 int speed_left =  200;
 int speed_right = 200;
+int imgNum = 0;
 void start();
 void forward();
 void reverse();
@@ -41,25 +68,105 @@
 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();
-    
-    testDistance();
-    wait(10);
-        playsong();
-
+    bool flag = true;
+    */
+//    testDistance();
+//    wait(10);
+//        playsong();
 // Move around with motor commands
     //forward();
 //    wait(.5);
@@ -79,7 +186,7 @@
 //    playsong();
 //    wait(10);
 // Search for battery charger IR beacon
-    charger();
+//    charger();
 }
  
  
@@ -142,7 +249,7 @@
 void testDistance() {
     int timer = 0;
     while(timer < 100000000) {
-        if(sensor > 0.75) {
+        if (sensor > 0.75) {
             led1 = 1;
             led2 = 1;
             led3 = 1;
@@ -186,4 +293,155 @@
         timer = timer + 1;
         timer = timer - 1;
     }
-}
\ No newline at end of file
+}
+
+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
+    
+}