ECE 4180 Project

Dependencies:   Camera_LS_Y201 SDFileSystem mbed

Fork of 4180_Final_Project by Paul Wilson

Files at this revision

API Documentation at this revision

Comitter:
lzzcd001
Date:
Thu Apr 30 19:01:22 2015 +0000
Parent:
0:21c11d775cc4
Commit message:
ECE 4180 Project

Changed in this revision

Camera_LS_Y201.lib Show annotated file Show diff for this revision Revisions of this file
HMC6352.cpp Show annotated file Show diff for this revision Revisions of this file
HMC6352.h Show annotated file Show diff for this revision Revisions of this file
SDFileSystem.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
robotControl.h Show annotated file Show diff for this revision Revisions of this file
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