Revision 1:dcd6c9be9e4b, committed 2015-04-30

Comitter:

lzzcd001

Date:

Thu Apr 30 19:01:22 2015 +0000

Parent:

0:21c11d775cc4

Commit message:

ECE 4180 Project

Changed in this revision

diff -r 21c11d775cc4 -r dcd6c9be9e4b Camera_LS_Y201.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Camera_LS_Y201.lib	Thu Apr 30 19:01:22 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/shintamainjp/code/Camera_LS_Y201/#43358d40f879

diff -r 21c11d775cc4 -r dcd6c9be9e4b HMC6352.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC6352.cpp	Thu Apr 30 19:01:22 2015 +0000
@@ -0,0 +1,270 @@
+/**
+ * @author Aaron Berk
+ * 
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * Honeywell HMC6352 digital compass.
+ *
+ * Datasheet:
+ *
+ * http://www.ssec.honeywell.com/magnetic/datasheets/HMC6352.pdf
+ */
+
+/**
+ * Includes
+ */
+#include "HMC6352.h"
+
+HMC6352::HMC6352(PinName sda, PinName scl) {
+
+    i2c_ = new I2C(sda, scl);
+    //100KHz, as specified by the datasheet.
+    i2c_->frequency(100000);
+
+    operationMode_ = getOpMode();
+
+}
+
+int HMC6352::sample(void) {
+
+    char tx[1];
+    char rx[2];
+
+    if (operationMode_ == HMC6352_STANDBY || operationMode_ == HMC6352_QUERY) {
+        tx[0] = HMC6352_GET_DATA;
+
+        i2c_->write((HMC6352_I2C_ADDRESS << 1) & 0xFE, tx, 1);
+        wait_ms(1);
+    }
+
+    i2c_->read((HMC6352_I2C_ADDRESS << 1) | 0x01, rx, 2);
+    wait_ms(1);
+
+    return (((int)rx[0] << 8) | (int)rx[1]);
+
+}
+
+void HMC6352::setSleepMode(int exitOrEnter) {
+
+    char tx[1];
+
+    tx[0] = exitOrEnter;
+
+    i2c_->write((HMC6352_I2C_ADDRESS << 1) & 0xFE, tx, 1);
+    wait_ms(1);
+
+}
+
+void HMC6352::setReset(void) {
+
+    char tx[1];
+
+    tx[0] = HMC6352_SET_RESET;
+
+    i2c_->write((HMC6352_I2C_ADDRESS << 1) & 0xFE, tx, 1);
+    wait_ms(7);
+
+}
+
+void HMC6352::setCalibrationMode(int exitOrEnter) {
+
+    char tx[1];
+    int delay = 0;
+
+    tx[0] = exitOrEnter;
+
+    if (exitOrEnter == HMC6352_EXIT_CALIB) {
+        delay = 15;
+    } else if (exitOrEnter == HMC6352_ENTER_CALIB) {
+        delay = 1;
+    }
+
+    i2c_->write((HMC6352_I2C_ADDRESS << 1) & 0xFE, tx, 1);
+    wait_ms(delay);
+
+}
+
+void HMC6352::saveOpMode(void) {
+
+    char tx[1];
+
+    tx[0] = HMC6352_SAVE_OPMODE;
+
+    i2c_->write((HMC6352_I2C_ADDRESS << 1) & 0xFE, tx, 1);
+    wait_ms(1);
+
+}
+
+int HMC6352::getSlaveAddress(void) {
+
+    return read(HMC6352_EEPROM_READ, HMC6352_SLAVE_ADDR);
+
+}
+
+int HMC6352::getOffset(int axis) {
+
+    char rx[2] = {0x00, 0x00};
+
+    if (axis == HMC6352_MX_OFFSET) {
+
+        rx[0] = read(HMC6352_EEPROM_READ, HMC6352_MX_OFF_MSB);
+        rx[1] = read(HMC6352_EEPROM_READ, HMC6352_MX_OFF_LSB);
+
+    } else if (axis == HMC6352_MY_OFFSET) {
+
+        rx[0] = read(HMC6352_EEPROM_READ, HMC6352_MY_OFF_MSB);
+        rx[1] = read(HMC6352_EEPROM_READ, HMC6352_MY_OFF_LSB);
+
+    }
+
+    return ((rx[0] << 8) | (rx[1]));
+
+}
+
+void HMC6352::setOffset(int axis, int offset) {
+
+    char tx[2] = {0x00, 0x00};
+
+    tx[0] = (offset & 0x0000FF00) >> 8;
+    tx[1] = (offset & 0x000000FF);
+
+    if (axis == HMC6352_MX_OFFSET) {
+
+        write(HMC6352_EEPROM_WRITE, HMC6352_MX_OFF_MSB, tx[0]);
+        write(HMC6352_EEPROM_WRITE, HMC6352_MX_OFF_MSB, tx[1]);
+
+    } else if (axis == HMC6352_MY_OFFSET) {
+
+        write(HMC6352_EEPROM_WRITE, HMC6352_MY_OFF_MSB, tx[0]);
+        write(HMC6352_EEPROM_WRITE, HMC6352_MY_OFF_MSB, tx[1]);
+
+    }
+
+}
+
+int HMC6352::getTimeDelay(void) {
+
+    return read(HMC6352_EEPROM_READ, HMC6352_TIME_DELAY);
+
+}
+
+void HMC6352::setTimeDelay(int delay) {
+
+    write(HMC6352_EEPROM_WRITE, HMC6352_TIME_DELAY, delay);
+
+}
+
+int HMC6352::getSumNumber(void) {
+
+    return read(HMC6352_EEPROM_READ, HMC6352_SUMMED);
+
+}
+
+void HMC6352::setSumNumber(int sum) {
+
+    write(HMC6352_EEPROM_WRITE, HMC6352_SUMMED, sum);
+
+}
+
+int HMC6352::getSoftwareVersion(void) {
+
+    return read(HMC6352_EEPROM_READ, HMC6352_SOFT_VER);
+
+}
+
+int HMC6352::getOpMode(void) {
+
+    int response = 0;
+
+    response = read(HMC6352_RAM_READ, HMC6352_RAM_OPMODE);
+
+    return (response & 0x00000003);
+
+}
+
+void HMC6352::setOpMode(int mode, int periodicSetReset, int measurementRate) {
+
+    char opModeByte = mode;
+
+    if (periodicSetReset == 1) {
+        opModeByte |= HMC6352_PERIODIC_SR;
+    }
+
+    if (measurementRate == 5) {
+        opModeByte |= HMC6352_CM_MR_5HZ;
+    } else if (measurementRate == 10) {
+        opModeByte |= HMC6352_CM_MR_10HZ;
+    } else if (measurementRate == 20) {
+        opModeByte |= HMC6352_CM_MR_20HZ;
+    }
+
+    write(HMC6352_RAM_WRITE, HMC6352_RAM_OPMODE, opModeByte);
+    write(HMC6352_EEPROM_WRITE, HMC6352_OPMODE, opModeByte);
+
+    operationMode_ = mode;
+
+}
+
+int HMC6352::getOutputMode(void) {
+
+    return read(HMC6352_RAM_READ, HMC6352_RAM_OUTPUT);
+
+}
+
+void HMC6352::setOutputMode(int mode) {
+
+    write(HMC6352_RAM_WRITE, HMC6352_RAM_OUTPUT, mode);
+
+}
+
+void HMC6352::write(int EepromOrRam, int address, int data) {
+
+    char tx[3];
+
+    tx[0] = EepromOrRam;
+    tx[1] = address;
+    tx[2] = data;
+
+    i2c_->write((HMC6352_I2C_ADDRESS << 1) & 0xFE, tx, 3);
+    wait_ms(1);
+
+}
+
+int HMC6352::read(int EepromOrRam, int address) {
+
+    char tx[2];
+    char rx[1];
+
+    tx[0] = EepromOrRam;
+    tx[1] = address;
+
+    i2c_->write((HMC6352_I2C_ADDRESS << 1) & 0xFE, tx, 2);
+    wait_ms(1);
+    i2c_->read((HMC6352_I2C_ADDRESS << 1) | 0x01, rx, 1);
+    wait_ms(1);
+
+    return (rx[0]);
+
+}

diff -r 21c11d775cc4 -r dcd6c9be9e4b HMC6352.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC6352.h	Thu Apr 30 19:01:22 2015 +0000
@@ -0,0 +1,283 @@
+/**
+ * @author Aaron Berk
+ * 
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * Honeywell HMC6352 digital compass.
+ *
+ * Datasheet:
+ *
+ * http://www.ssec.honeywell.com/magnetic/datasheets/HMC6352.pdf
+ */
+
+#ifndef HMC6352_H
+#define HMC6352_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+/**
+ * Defines
+ */
+#define HMC6352_I2C_ADDRESS  0x21 //7-bit address. 0x43 write, 0x42 read.
+
+//Commands.
+#define HMC6352_EEPROM_WRITE 0x77
+#define HMC6352_EEPROM_READ  0x72
+#define HMC6352_RAM_WRITE    0x47
+#define HMC6352_RAM_READ     0x67
+#define HMC6352_ENTER_SLEEP  0x53
+#define HMC6352_EXIT_SLEEP   0x57
+#define HMC6352_SET_RESET    0x4F
+#define HMC6352_ENTER_CALIB  0x43
+#define HMC6352_EXIT_CALIB   0x45
+#define HMC6352_SAVE_OPMODE  0x4C
+#define HMC6352_GET_DATA     0x41
+
+//EEPROM locations.
+#define HMC6352_SLAVE_ADDR   0x00
+#define HMC6352_MX_OFF_MSB   0x01
+#define HMC6352_MX_OFF_LSB   0x02
+#define HMC6352_MY_OFF_MSB   0x03
+#define HMC6352_MY_OFF_LSB   0x04
+#define HMC6352_TIME_DELAY   0x05
+#define HMC6352_SUMMED       0x06
+#define HMC6352_SOFT_VER     0x07
+#define HMC6352_OPMODE       0x08
+
+//RAM registers.
+#define HMC6352_RAM_OPMODE   0x74
+#define HMC6352_RAM_OUTPUT   0x4E
+
+#define HMC6352_MX_OFFSET    0x00
+#define HMC6352_MY_OFFSET    0x01
+
+#define HMC6352_HEADING_MODE 0x00
+#define HMC6352_RAWMAGX_MODE 0x01
+#define HMC6352_RAWMAGY_MODE 0x02
+#define HMC6352_MAGX_MODE    0x03
+#define HMC6352_MAGY_MODE    0x04
+
+//Operational mode register masks.
+#define HMC6352_CM_MR_1HZ    0x00
+#define HMC6352_CM_MR_5HZ    0x20
+#define HMC6352_CM_MR_10HZ   0x40
+#define HMC6352_CM_MR_20HZ   0x60
+
+#define HMC6352_PERIODIC_SR  0x10
+
+#define HMC6352_STANDBY      0x00
+#define HMC6352_QUERY        0x01
+#define HMC6352_CONTINUOUS   0x02
+
+/**
+ * Honeywell HMC6352 digital compass.
+ */
+class HMC6352 {
+
+public:
+
+    /**
+     * Constructor.
+     *
+     * @param sda mbed pin to use for SDA line of I2C interface.
+     * @param scl mbed pin to use for SCL line of I2C interface.
+     */
+    HMC6352(PinName sda, PinName scl);
+
+    /**
+     * Sample the device and return the result.
+     *
+     * @return In heading output mode, the current heading as a number between
+     *         0-3599, representing 0-359.9 degrees.
+     *         In raw magnetometer X output mode, the raw output of the X-axis
+     *         magnetometer.
+     *         In raw magnetometer Y mode, the raw output of the Y-axis
+     *         magnetometer.
+     *         In magnetometer X mode, the corrected output of the X-axis
+     *         magnetometer.
+     *         In magnetometer Y mode, the corrected output of the Y-axis
+     *         magnetometer.
+     */
+    int sample(void);
+
+    /**
+     * Enter into or exit from sleep mode.
+     *
+     * @param enterOrExit 0x57 -> Exit sleep mode
+     *                    0x53 -> Enter
+     */
+    void setSleepMode(int enterOrExit);
+
+    /**
+     * Update bridge offsets.
+     *
+     * Performs a set/reset immediately.
+     */
+    void setReset(void);
+
+    /**
+     * Enter into or exit from calibration mode.
+     *
+     * @param enterOrExit 0x45 -> Exit
+     *                    0x43 -> Enter
+     */
+    void setCalibrationMode(int enterOrExit);
+
+    /**
+     * Save the current operation mode byte to EEPROM.
+     */
+    void saveOpMode(void);
+
+    /**
+     * Read the memory location on the device which contains the slave address.
+     *
+     * @return The slave address of the device.
+     */
+    int getSlaveAddress(void);
+
+    /**
+     * Read the current offset for X or Y axis magnetometer.
+     *
+     * @param axis 0x00 -> X-axis
+     *             0x01 -> Y-axis
+     * @return The current offset for the axis as a 16-bit number.
+     */
+    int getOffset(int axis);
+
+    /**
+     * Set the offset for X or Y axis magnetometer.
+     *
+     * @param axis 0x00 -> X-axis
+     *             0x01 -> Y-axis
+     * @param offset The offset to set for the axis (set to a 16-bit number).
+     */
+    void setOffset(int axis, int offset);
+
+    /**
+     * Get the current time delay.
+     *
+     * @return The time delay in milliseconds.
+     */
+    int getTimeDelay(void);
+
+    /**
+     * Set the time delay of the device.
+     *
+     * @param delay The time delay to set in milliseconds (between 0-255);
+     */
+    void setTimeDelay(int delay);
+
+    /**
+     * Get the number of measurements being summed each sample.
+     *
+     * @return The number of summed measurement.
+     */
+    int getSumNumber(void);
+
+    /**
+     * Set the number of measurements being summed each sample.
+     *
+     * @param sum The number of measurements to be summed (between 0-16);
+     */
+    void setSumNumber(int sum);
+
+    /**
+     * Get the software version on the device.
+     *
+     * @return The software version number.
+     */
+    int getSoftwareVersion(void);
+
+    /**
+     * Get the current operation mode.
+     *
+     * @return 0x00 -> Standby mode
+     *         0x01 -> Query mode
+     *         0x02 -> Continuous mode
+     */
+    int getOpMode(void);
+
+    /**
+     * Set the operation mode.
+     *
+     * @param mode 0x00 -> Standby mode
+     *             0x01 -> Query mode
+     *             0x02 -> Continuous mode
+     * @param periodicSetReset 0x00 -> No periodic set/reset
+     *                         0x01 -> Periodic set/reset
+     * @measurementRate Measurement rate in Hz for continuous rate.
+     *                  Possible rates: {1, 5, 10, 20}Hz.
+     */
+    void setOpMode(int mode, int periodicSetReset, int measurementRate = 0);
+
+    /**
+     * Get the current output mode of the device.
+     *
+     * @return The current output mode.
+     */
+    int getOutputMode(void);
+
+    /**
+     * Set the output mode of the device.
+     *
+     * @param mode 0x00 -> Heading mode
+     *             0x01 -> Raw magnetometer X mode
+     *             0x02 -> Raw magnetometer Y mode
+     *             0x03 -> Magnetometer X mode
+     *             0x04 -> Magnetometer Y mode
+     */
+    void setOutputMode(int mode);
+
+private:
+
+    I2C* i2c_;
+    int  operationMode_;
+
+    /**
+     * Write to EEPROM or RAM on the device.
+     *
+     * @param EepromOrRam 0x77 -> Writing to EEPROM
+     *                    0x47 -> Writing to RAM
+     * @param address Address to write to.
+     * @param data Data to write.
+     */
+    void write(int EepromOrRam, int address, int data);
+
+    /**
+     * Read EEPROM or RAM on the device.
+     *
+     * @param EepromOrRam 0x72 -> Reading from EEPROM
+     *                    0x67 -> Reading from RAM
+     * @param address Address to read from.
+     * @return The contents of the memory address.
+     */
+    int read(int EepromOrRam, int address);
+
+};
+
+#endif /* HMC6352_H */

diff -r 21c11d775cc4 -r dcd6c9be9e4b SDFileSystem.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.lib	Thu Apr 30 19:01:22 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/SDFileSystem/#c8f66dc765d4

diff -r 21c11d775cc4 -r dcd6c9be9e4b main.cpp
--- 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
+    
+}

diff -r 21c11d775cc4 -r dcd6c9be9e4b robotControl.h