Revision 0:ee786e500a3c, committed 2010-12-17

Comitter:

sowmy87

Date:

Fri Dec 17 18:27:33 2010 +0000

Child:

1:2bac7b6f3840

Commit message:

Completed

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CMPS03.lib	Fri Dec 17 18:27:33 2010 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/CMPS03/#c6bcc390612a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FATFileSystem.lib	Fri Dec 17 18:27:33 2010 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_unsupported/code/fatfilesystem/
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.cpp	Fri Dec 17 18:27:33 2010 +0000
@@ -0,0 +1,457 @@
+/* mbed SDFileSystem Library, for providing file access to SD cards
+ * Copyright (c) 2008-2010, sford
+ *
+ * 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.
+ */
+
+/* Introduction
+ * ------------
+ * SD and MMC cards support a number of interfaces, but common to them all
+ * is one based on SPI. This is the one I'm implmenting because it means
+ * it is much more portable even though not so performant, and we already 
+ * have the mbed SPI Interface!
+ *
+ * The main reference I'm using is Chapter 7, "SPI Mode" of: 
+ *  http://www.sdcard.org/developers/tech/sdcard/pls/Simplified_Physical_Layer_Spec.pdf
+ *
+ * SPI Startup
+ * -----------
+ * The SD card powers up in SD mode. The SPI interface mode is selected by
+ * asserting CS low and sending the reset command (CMD0). The card will 
+ * respond with a (R1) response.
+ *
+ * CMD8 is optionally sent to determine the voltage range supported, and 
+ * indirectly determine whether it is a version 1.x SD/non-SD card or 
+ * version 2.x. I'll just ignore this for now.
+ *
+ * ACMD41 is repeatedly issued to initialise the card, until "in idle"
+ * (bit 0) of the R1 response goes to '0', indicating it is initialised.
+ *
+ * You should also indicate whether the host supports High Capicity cards,
+ * and check whether the card is high capacity - i'll also ignore this
+ *
+ * SPI Protocol
+ * ------------
+ * The SD SPI protocol is based on transactions made up of 8-bit words, with
+ * the host starting every bus transaction by asserting the CS signal low. The
+ * card always responds to commands, data blocks and errors.
+ * 
+ * The protocol supports a CRC, but by default it is off (except for the 
+ * first reset CMD0, where the CRC can just be pre-calculated, and CMD8)
+ * I'll leave the CRC off I think! 
+ * 
+ * Standard capacity cards have variable data block sizes, whereas High 
+ * Capacity cards fix the size of data block to 512 bytes. I'll therefore
+ * just always use the Standard Capacity cards with a block size of 512 bytes.
+ * This is set with CMD16.
+ *
+ * You can read and write single blocks (CMD17, CMD25) or multiple blocks 
+ * (CMD18, CMD25). For simplicity, I'll just use single block accesses. When
+ * the card gets a read command, it responds with a response token, and then 
+ * a data token or an error.
+ * 
+ * SPI Command Format
+ * ------------------
+ * Commands are 6-bytes long, containing the command, 32-bit argument, and CRC.
+ *
+ * +---------------+------------+------------+-----------+----------+--------------+
+ * | 01 | cmd[5:0] | arg[31:24] | arg[23:16] | arg[15:8] | arg[7:0] | crc[6:0] | 1 |
+ * +---------------+------------+------------+-----------+----------+--------------+
+ *
+ * As I'm not using CRC, I can fix that byte to what is needed for CMD0 (0x95)
+ *
+ * All Application Specific commands shall be preceded with APP_CMD (CMD55).
+ *
+ * SPI Response Format
+ * -------------------
+ * The main response format (R1) is a status byte (normally zero). Key flags:
+ *  idle - 1 if the card is in an idle state/initialising 
+ *  cmd  - 1 if an illegal command code was detected
+ *
+ *    +-------------------------------------------------+
+ * R1 | 0 | arg | addr | seq | crc | cmd | erase | idle |
+ *    +-------------------------------------------------+
+ *
+ * R1b is the same, except it is followed by a busy signal (zeros) until
+ * the first non-zero byte when it is ready again.
+ *
+ * Data Response Token
+ * -------------------
+ * Every data block written to the card is acknowledged by a byte 
+ * response token
+ *
+ * +----------------------+
+ * | xxx | 0 | status | 1 |
+ * +----------------------+
+ *              010 - OK!
+ *              101 - CRC Error
+ *              110 - Write Error
+ *
+ * Single Block Read and Write
+ * ---------------------------
+ *
+ * Block transfers have a byte header, followed by the data, followed
+ * by a 16-bit CRC. In our case, the data will always be 512 bytes.
+ *  
+ * +------+---------+---------+- -  - -+---------+-----------+----------+
+ * | 0xFE | data[0] | data[1] |        | data[n] | crc[15:8] | crc[7:0] | 
+ * +------+---------+---------+- -  - -+---------+-----------+----------+
+ */
+ 
+#include "SDFileSystem.h"
+
+#define SD_COMMAND_TIMEOUT 5000
+
+SDFileSystem::SDFileSystem(PinName mosi, PinName miso, PinName sclk, PinName cs, const char* name) :
+  FATFileSystem(name), _spi(mosi, miso, sclk), _cs(cs) {
+      _cs = 1; 
+}
+
+#define R1_IDLE_STATE           (1 << 0)
+#define R1_ERASE_RESET          (1 << 1)
+#define R1_ILLEGAL_COMMAND      (1 << 2)
+#define R1_COM_CRC_ERROR        (1 << 3)
+#define R1_ERASE_SEQUENCE_ERROR (1 << 4)
+#define R1_ADDRESS_ERROR        (1 << 5)
+#define R1_PARAMETER_ERROR      (1 << 6)
+
+// Types
+//  - v1.x Standard Capacity
+//  - v2.x Standard Capacity
+//  - v2.x High Capacity
+//  - Not recognised as an SD Card
+
+#define SDCARD_FAIL 0
+#define SDCARD_V1   1
+#define SDCARD_V2   2
+#define SDCARD_V2HC 3
+
+int SDFileSystem::initialise_card() {
+    // Set to 100kHz for initialisation, and clock card with cs = 1
+    _spi.frequency(100000); 
+    _cs = 1;
+    for(int i=0; i<16; i++) {   
+        _spi.write(0xFF);
+    }
+
+    // send CMD0, should return with all zeros except IDLE STATE set (bit 0)
+    if(_cmd(0, 0) != R1_IDLE_STATE) { 
+        fprintf(stderr, "No disk, or could not put SD card in to SPI idle state\n");
+        return SDCARD_FAIL;
+    }
+
+    // send CMD8 to determine whther it is ver 2.x
+    int r = _cmd8();
+    if(r == R1_IDLE_STATE) {
+        return initialise_card_v2();
+    } else if(r == (R1_IDLE_STATE | R1_ILLEGAL_COMMAND)) {
+        return initialise_card_v1();
+    } else {
+        fprintf(stderr, "Not in idle state after sending CMD8 (not an SD card?)\n");
+        return SDCARD_FAIL;
+    }
+}
+
+int SDFileSystem::initialise_card_v1() {
+    for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+        _cmd(55, 0); 
+        if(_cmd(41, 0) == 0) { 
+            return SDCARD_V1;
+        }
+    }
+
+    fprintf(stderr, "Timeout waiting for v1.x card\n");
+    return SDCARD_FAIL;
+}
+
+int SDFileSystem::initialise_card_v2() {
+    
+    for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+        _cmd(55, 0); 
+        if(_cmd(41, 0) == 0) { 
+            _cmd58();
+            return SDCARD_V2;
+        }
+    }
+
+    fprintf(stderr, "Timeout waiting for v2.x card\n");
+    return SDCARD_FAIL;
+}
+
+int SDFileSystem::disk_initialize() {
+
+    int i = initialise_card();
+//    printf("init card = %d\n", i);
+//    printf("OK\n");
+
+    _sectors = _sd_sectors();
+
+    // Set block length to 512 (CMD16)
+    if(_cmd(16, 512) != 0) {
+        fprintf(stderr, "Set 512-byte block timed out\n");
+        return 1;
+    }
+        
+    _spi.frequency(1000000); // Set to 1MHz for data transfer
+    return 0;
+}
+
+int SDFileSystem::disk_write(const char *buffer, int block_number) {
+    // set write address for single block (CMD24)
+    if(_cmd(24, block_number * 512) != 0) {
+        return 1;
+    }
+
+    // send the data block
+    _write(buffer, 512);    
+    return 0;    
+}
+
+int SDFileSystem::disk_read(char *buffer, int block_number) {        
+    // set read address for single block (CMD17)
+    if(_cmd(17, block_number * 512) != 0) {
+        return 1;
+    }
+    
+    // receive the data
+    _read(buffer, 512);
+    return 0;
+}
+
+int SDFileSystem::disk_status() { return 0; }
+int SDFileSystem::disk_sync() { return 0; }
+int SDFileSystem::disk_sectors() { return _sectors; }
+
+// PRIVATE FUNCTIONS
+
+int SDFileSystem::_cmd(int cmd, int arg) {
+    _cs = 0; 
+
+    // send a command
+    _spi.write(0x40 | cmd);
+    _spi.write(arg >> 24);
+    _spi.write(arg >> 16);
+    _spi.write(arg >> 8);
+    _spi.write(arg >> 0);
+    _spi.write(0x95);
+
+    // wait for the repsonse (response[7] == 0)
+    for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+        int response = _spi.write(0xFF);
+        if(!(response & 0x80)) {
+            _cs = 1;
+            _spi.write(0xFF);
+            return response;
+        }
+    }
+    _cs = 1;
+    _spi.write(0xFF);
+    return -1; // timeout
+}
+int SDFileSystem::_cmdx(int cmd, int arg) {
+    _cs = 0; 
+
+    // send a command
+    _spi.write(0x40 | cmd);
+    _spi.write(arg >> 24);
+    _spi.write(arg >> 16);
+    _spi.write(arg >> 8);
+    _spi.write(arg >> 0);
+    _spi.write(0x95);
+
+    // wait for the repsonse (response[7] == 0)
+    for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+        int response = _spi.write(0xFF);
+        if(!(response & 0x80)) {
+            return response;
+        }
+    }
+    _cs = 1;
+    _spi.write(0xFF);
+    return -1; // timeout
+}
+
+
+int SDFileSystem::_cmd58() {
+    _cs = 0; 
+    int arg = 0;
+    
+    // send a command
+    _spi.write(0x40 | 58);
+    _spi.write(arg >> 24);
+    _spi.write(arg >> 16);
+    _spi.write(arg >> 8);
+    _spi.write(arg >> 0);
+    _spi.write(0x95);
+
+    // wait for the repsonse (response[7] == 0)
+    for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+        int response = _spi.write(0xFF);
+        if(!(response & 0x80)) {
+            int ocr = _spi.write(0xFF) << 24;
+            ocr |= _spi.write(0xFF) << 16;
+            ocr |= _spi.write(0xFF) << 8;
+            ocr |= _spi.write(0xFF) << 0;
+//            printf("OCR = 0x%08X\n", ocr);
+            _cs = 1;
+            _spi.write(0xFF);
+            return response;
+        }
+    }
+    _cs = 1;
+    _spi.write(0xFF);
+    return -1; // timeout
+}
+
+int SDFileSystem::_cmd8() {
+    _cs = 0; 
+    
+    // send a command
+    _spi.write(0x40 | 8); // CMD8
+    _spi.write(0x00);     // reserved
+    _spi.write(0x00);     // reserved
+    _spi.write(0x01);     // 3.3v
+    _spi.write(0xAA);     // check pattern
+    _spi.write(0x87);     // crc
+
+    // wait for the repsonse (response[7] == 0)
+    for(int i=0; i<SD_COMMAND_TIMEOUT * 1000; i++) {
+        char response[5];
+        response[0] = _spi.write(0xFF);
+        if(!(response[0] & 0x80)) {
+                for(int j=1; j<5; j++) {
+                    response[i] = _spi.write(0xFF);
+                }
+                _cs = 1;
+                _spi.write(0xFF);
+                return response[0];
+        }
+    }
+    _cs = 1;
+    _spi.write(0xFF);
+    return -1; // timeout
+}
+
+int SDFileSystem::_read(char *buffer, int length) {
+    _cs = 0;
+
+    // read until start byte (0xFF)
+    while(_spi.write(0xFF) != 0xFE);
+
+    // read data
+    for(int i=0; i<length; i++) {
+        buffer[i] = _spi.write(0xFF);
+    }
+    _spi.write(0xFF); // checksum
+    _spi.write(0xFF);
+
+    _cs = 1;    
+    _spi.write(0xFF);
+    return 0;
+}
+
+int SDFileSystem::_write(const char *buffer, int length) {
+    _cs = 0;
+    
+    // indicate start of block
+    _spi.write(0xFE);
+    
+    // write the data
+    for(int i=0; i<length; i++) {
+        _spi.write(buffer[i]);
+    }
+    
+    // write the checksum
+    _spi.write(0xFF); 
+    _spi.write(0xFF);
+
+    // check the repsonse token
+    if((_spi.write(0xFF) & 0x1F) != 0x05) {
+        _cs = 1;
+        _spi.write(0xFF);        
+        return 1;
+    }
+
+    // wait for write to finish
+    while(_spi.write(0xFF) == 0);
+
+    _cs = 1; 
+    _spi.write(0xFF);
+    return 0;
+}
+
+static int ext_bits(char *data, int msb, int lsb) {
+    int bits = 0;
+    int size = 1 + msb - lsb; 
+    for(int i=0; i<size; i++) {
+        int position = lsb + i;
+        int byte = 15 - (position >> 3);
+        int bit = position & 0x7;
+        int value = (data[byte] >> bit) & 1;
+        bits |= value << i;
+    }
+    return bits;
+}
+
+int SDFileSystem::_sd_sectors() {
+
+    // CMD9, Response R2 (R1 byte + 16-byte block read)
+    if(_cmdx(9, 0) != 0) {
+        fprintf(stderr, "Didn't get a response from the disk\n");
+        return 0;
+    }
+    
+    char csd[16];    
+    if(_read(csd, 16) != 0) {
+        fprintf(stderr, "Couldn't read csd response from disk\n");
+        return 0;
+    }
+
+    // csd_structure : csd[127:126]
+    // c_size        : csd[73:62]
+    // c_size_mult   : csd[49:47]
+    // read_bl_len   : csd[83:80] - the *maximum* read block length
+
+    int csd_structure = ext_bits(csd, 127, 126);
+    int c_size = ext_bits(csd, 73, 62);
+    int c_size_mult = ext_bits(csd, 49, 47);
+    int read_bl_len = ext_bits(csd, 83, 80);
+
+//    printf("CSD_STRUCT = %d\n", csd_structure);
+    
+    if(csd_structure != 0) {
+        fprintf(stderr, "This disk tastes funny! I only know about type 0 CSD structures\n");
+        return 0;
+    }
+             
+    // memory capacity = BLOCKNR * BLOCK_LEN
+    // where
+    //  BLOCKNR = (C_SIZE+1) * MULT
+    //  MULT = 2^(C_SIZE_MULT+2) (C_SIZE_MULT < 8)
+    //  BLOCK_LEN = 2^READ_BL_LEN, (READ_BL_LEN < 12)         
+                            
+    int block_len = 1 << read_bl_len;
+    int mult = 1 << (c_size_mult + 2);
+    int blocknr = (c_size + 1) * mult;
+    int capacity = blocknr * block_len;
+        
+    int blocks = capacity / 512;
+        
+    return blocks;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.h	Fri Dec 17 18:27:33 2010 +0000
@@ -0,0 +1,81 @@
+/* mbed SDFileSystem Library, for providing file access to SD cards
+ * Copyright (c) 2008-2010, sford
+ *
+ * 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.
+ */
+
+#ifndef MBED_SDFILESYSTEM_H
+#define MBED_SDFILESYSTEM_H
+
+#include "mbed.h"
+#include "FATFileSystem.h"
+
+/** Access the filesystem on an SD Card using SPI
+ *
+ * @code
+ * #include "mbed.h"
+ * #include "SDFileSystem.h"
+ *
+ * SDFileSystem sd(p5, p6, p7, p12, "sd"); // mosi, miso, sclk, cs
+ *  
+ * int main() {
+ *     FILE *fp = fopen("/sd/myfile.txt", "w");
+ *     fprintf(fp, "Hello World!\n");
+ *     fclose(fp);
+ * }
+ */
+class SDFileSystem : public FATFileSystem {
+public:
+
+    /** Create the File System for accessing an SD Card using SPI
+     *
+     * @param mosi SPI mosi pin connected to SD Card
+     * @param miso SPI miso pin conencted to SD Card
+     * @param sclk SPI sclk pin connected to SD Card
+     * @param cs   DigitalOut pin used as SD Card chip select
+     * @param name The name used to access the virtual filesystem
+     */
+    SDFileSystem(PinName mosi, PinName miso, PinName sclk, PinName cs, const char* name);
+    virtual int disk_initialize();
+    virtual int disk_write(const char *buffer, int block_number);
+    virtual int disk_read(char *buffer, int block_number);    
+    virtual int disk_status();
+    virtual int disk_sync();
+    virtual int disk_sectors();
+
+protected:
+
+    int _cmd(int cmd, int arg);
+    int _cmdx(int cmd, int arg);
+    int _cmd8();
+    int _cmd58();
+    int initialise_card();
+    int initialise_card_v1();
+    int initialise_card_v2();
+    
+    int _read(char *buffer, int length);
+    int _write(const char *buffer, int length);
+    int _sd_sectors();
+    int _sectors;
+    
+    SPI _spi;
+    DigitalOut _cs;     
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SRF05.lib	Fri Dec 17 18:27:33 2010 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/SRF05/#e758665e072c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Serializer/Serializer.cpp	Fri Dec 17 18:27:33 2010 +0000
@@ -0,0 +1,330 @@
+/**
+ * Includes
+ */
+#include "Serializer.h"
+
+//Serial pcc(USBTX, USBRX);
+
+Serializer::Serializer() {
+    serial=new Serial(p28,p27);
+    serial->baud(BAUD);
+    Initialize();
+}
+
+int Serializer::Initialize() {
+    char c1=0;
+    DigitalOut led1(LED1);
+    DigitalOut led2(LED2);
+    DigitalOut led3(LED3);
+    DigitalOut led4(LED4);
+
+    led1=1;
+    if (serial->writeable())
+        serial->printf("fw\r");
+    wait(.25);
+    led2=1;
+    while (serial->readable()) {
+        c1=serial->getc();
+    }
+    if (c1!='>')
+        for (int i=0;i<5;i++) {
+            led1=led2=led3=led4=1;
+            wait(0.1);
+            led1=led2=led3=led4=0;
+            wait(0.1);
+        }
+    wait(.25);
+    led3=1;
+    wait(.25);
+    led4=1;
+ //   SetVPID(1,0,0,100);
+    wait(0.25);
+    led1=0;
+    wait(.25);
+    ClearCount();
+    led2=0;
+    wait(.25);
+    led3=0;
+    wait(.25);
+    led4=0;
+    _lPWM=_rPWM=0;
+    _isBusy=0;
+    _irqIsBusy=0;
+//    serial->attach(this, &Serializer::InterruptHandler);
+
+    return 0;
+}
+
+Serializer::~Serializer() {
+    delete serial;
+}
+
+void Serializer::Stop() {
+    if (serial->writeable())
+        serial->printf("stop\r");
+    leftSpeed=rightSpeed=0;
+//    pc.printf("Stop\r\n");
+}
+
+void Serializer::ClearCountLeft() {
+    if (serial->writeable())
+        serial->printf("clrenc 1\r");
+}
+void Serializer::ClearCountRight() {
+    if (serial->writeable())
+        serial->printf("clrenc 2\r");
+}
+
+void Serializer::ClearCount() {
+    if (serial->writeable())
+        serial->printf("clrenc 1 2\r");
+}
+
+void Serializer::SetSpeedLeft(int inPsec) {
+    if (inPsec>MAX_SPEED)inPsec=MAX_SPEED;
+    if (inPsec<-MAX_SPEED)inPsec=-MAX_SPEED;
+    if (serial->writeable())
+        serial->printf("mogo 1:%i\r", -inPsec);
+    leftSpeed=inPsec;
+}
+void Serializer::SetSpeedRight(int inPsec) {
+    if (inPsec>MAX_SPEED)inPsec=MAX_SPEED;
+    if (inPsec<-MAX_SPEED)inPsec=-MAX_SPEED;
+    if (serial->writeable())
+        serial->printf("mogo 2:%i\r", -inPsec);
+    rightSpeed=inPsec;
+}
+
+void Serializer::SetSpeed(int inPsec) {
+    if (inPsec>MAX_SPEED)inPsec=MAX_SPEED;
+    if (inPsec<-MAX_SPEED)inPsec=-MAX_SPEED;
+    inPsec*=-1;
+    if (serial->writeable())
+        serial->printf("mogo 1:%i 2:%i\r", inPsec-50, inPsec);
+    leftSpeed=rightSpeed=inPsec;
+}
+
+void Serializer::SetVPID(int p,int i,int d,int l) {
+    if (serial->writeable())
+        serial->printf("vpid %i:%i:%i:%i\r", p,i,d,l);
+}
+
+void Serializer::SetDPID(int p,int i,int d,int a) {
+    if (serial->writeable())
+        serial->printf("dpid %i:%i:%i:%i\r", p,i,d,a);
+}
+
+void Serializer::DiGoLeft(int dist,int inPsec) {
+/*    if(_isBusy)
+        return;
+    _isBusy=1;
+*/
+    if (serial->writeable())
+        serial->printf("digo 1:%i:%i\r",dist*PULSES_PER_INCH, inPsec);
+}
+void Serializer::DiGoRight(int dist,int inPsec) {
+/*     if(_isBusy)
+        return;
+    _isBusy=1;
+*/
+    if (serial->writeable())
+        serial->printf("digo 2:%i:%i\r",dist*PULSES_PER_INCH, inPsec);
+}
+void Serializer::DiGo(int dist,int inPsec) {
+/*    if(_isBusy)
+        return;
+    _isBusy=1;
+*/
+    if (serial->writeable()) {
+        serial->printf("digo 1:%i:%i 2:%i:%i\r", \
+                       -dist*PULSES_PER_INCH,   \
+                       inPsec,                  \
+                       -dist*PULSES_PER_INCH,   \
+                       inPsec);
+
+    }
+}
+
+int Serializer::IsBusy() {
+
+    if (_isBusy&&serial->writeable()) {
+        serial->printf("pids\r");
+        wait(0.01);
+    }
+    return _isBusy;
+}
+/*
+      char tmp=0;
+        if (_isBusy) {
+            if (serial->writeable()) {
+                serial->printf("pids\r");
+            }
+            wait(0.1);
+            while (serial->readable()) {
+                tmp=serial->getc();
+                pcc.putc(tmp);
+                if (tmp=='1')
+                    _isBusy=1;
+                else if (tmp=='0')
+                    _isBusy=0;
+            }
+        }
+        pcc.printf("IsBusy = %i\n\r",_isBusy);
+        return _isBusy;
+*/
+
+
+
+void Serializer::TurnLeft(int deg) {
+
+}
+
+void Serializer::TurnRight(int deg) {
+
+
+}
+void Serializer::PivetLeft(int deg) {
+
+    Stop();
+    deg=deg*PIVET_ADJUSTMENT;
+    wait(0.1);
+    if (serial->writeable())
+        serial->printf("digo 1:%i:%i 2:%i:%i\r",deg,PIVET_SPEED,-deg,PIVET_SPEED);
+
+}
+void Serializer::PivetRight(int deg) {
+    this->Stop();
+    deg=deg*PIVET_ADJUSTMENT;
+    wait(0.1);
+    if (serial->writeable())
+        serial->printf("digo 1:%i:%i 2:%i:%i\r",-deg,PIVET_SPEED,deg,PIVET_SPEED);
+
+}
+
+void Serializer::SetLeftPWM(int pwm) {
+    _lPWM=-pwm;
+    if (serial->writeable())
+        serial->printf("pwm 1:%i\r",_lPWM);
+
+}
+
+void Serializer::SetRightPWM(int pwm) {
+    _rPWM=-pwm;
+    if (serial->writeable())
+        serial->printf("pwm 2:%i\r",_rPWM);
+}
+
+void Serializer::SetPWM(int pwm){
+    _rPWM=-pwm;
+    _lPWM=-pwm;
+    if (serial->writeable())
+        serial->printf("pwm 1:%i 2:%i\r",_lPWM, _rPWM);      
+ }
+
+void Serializer::SetPWM(int lPwm, int rPwm){
+    _rPWM=-rPwm;
+    _lPWM=-lPwm;
+    if (serial->writeable())
+        serial->printf("pwm 1:%i 2:%i\r",_lPWM, _rPWM);      
+ }
+
+int Serializer::GetCountLeft() {
+    if (serial->writeable()) {
+        serial->printf("getenc 1\r");
+        //        pcc.printf("Sendreq\t");
+    }
+    wait(0.01);
+    int ret=0;
+    char c=0;
+    while (serial->readable()) {
+        //        pcc.printf("Reading");
+        c=serial->getc();
+        //        pcc.putc(c);
+        if (c>='0'&&c<='9') {
+            c-='0';
+            ret*=10;
+            ret+=c;
+        }
+    }
+    return ret;
+}
+
+int Serializer::GetCountRight() {
+    if (serial->writeable()) {
+        serial->printf("getenc 2\r");
+        //        pcc.printf("Sendreq\t");
+    }
+    wait(0.01);
+    int ret=0;
+    char c=0;
+    while (serial->readable()) {
+        //        pcc.printf("Reading");
+        c=serial->getc();
+        //        pcc.putc(c);
+        if (c>='0'&&c<='9') {
+            c-='0';
+            ret*=10;
+            ret+=c;
+        }
+    }
+    return ret;
+}
+int Serializer::GetCount() {
+    return GetCountLeft();
+}
+float Serializer::GetDistanceLeft() {
+    return float(GetCountLeft()*PULSE_DISTANCE);
+}
+float Serializer::GetDistanceRight() {
+    return float(GetCountRight()*PULSE_DISTANCE);
+}
+float Serializer::GetDistance() {
+    return float(GetCountLeft()*PULSE_DISTANCE);
+}
+
+
+//unused and untested
+int Serializer::GetReply() {
+    int returnValue=0;
+    char c=0;
+    char readyToReturn=0;
+    char ack=0;
+    char nack=0;
+    wait(0.0001);
+    while (serial->readable()) {
+        c=serial->getc();
+        if (readyToReturn)continue;
+        if (c=='N'&&nack==0)nack=1;
+        else nack=0;
+        if (c=='A'&&ack==0)ack++;
+        else ack=nack=0;
+        if (c=='C'&&ack==1)ack++;
+        else ack=nack=0;
+        if (c=='K'&&ack==2)ack++;
+        else ack=nack=0;
+        if (ack==3)
+            if (nack==1)
+                returnValue=1;
+            else
+                returnValue=0;
+        if (c=='>')readyToReturn=1;
+    }
+    return returnValue;
+}
+
+
+void Serializer::InterruptHandler() {
+    char c=0;
+    char bsy=_isBusy;
+    while (serial->readable()) {
+        c=serial->getc();
+        if (!_isBusy)
+            continue;
+
+        if (c=='1')
+            bsy=1;
+        else if (c=='0')
+            bsy=0;
+    }
+    if (_isBusy&&!bsy) _isBusy=0;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Serializer/Serializer.h	Fri Dec 17 18:27:33 2010 +0000
@@ -0,0 +1,154 @@
+#ifndef SERIALIZER_H
+#define SERIALIZER_H
+
+
+#define     BAUD                    19200
+#define     MAX_SPEED               127
+
+#define     WAIT                    .001
+#define     PI                      3.1415926
+#define     PIVET_ADJUSTMENT        7.4
+#define     PIVET_SPEED             20
+
+#define     PULSES_PER_REVOLUTION   624
+#define     WHEEL_DIAMETER          2.25           //inches
+#define     WHEEL_CIRCUMFERENCE     PI*WHEEL_DIAMETER
+#define     PULSE_DISTANCE          WHEEL_CIRCUMFERENCE / 624
+#define     PULSES_PER_INCH         int(PULSES_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)
+
+
+#include "mbed.h"
+
+/**
+ * Serializer Class to communicate with Robotics
+ * Connection Serializer(tm) board
+ */
+class Serializer {
+
+public:
+    /**
+     * Constructor.
+     *
+     */
+    Serializer();
+
+    /**
+     * Destructor.
+     *
+     */
+    ~Serializer();
+
+    /**
+     * Clears left motor encoder count
+     *
+     */
+    void    ClearCountLeft();
+
+    /**
+     * Clears right motor encoder count
+     *
+     */
+    void    ClearCountRight();
+
+    /**
+     * Clears motors encoder counts
+     *
+     */
+    void    ClearCount();
+
+    /**
+     * Sets left motor speed
+     * @param inPsec Motor Speed in inches per second
+     */
+    void    SetSpeedLeft(int inPsec);
+
+    /**
+     * Sets right motor speed
+     * @param inPsec Motor Speed in inches per second
+     */
+    void    SetSpeedRight(int inPsec);
+
+    /**
+     * Sets speed for both motors
+     * @param inPsec Motor Speed in inches per second
+     */
+    void    SetSpeed(int inPsec);
+
+    /**
+     * Sets VPID
+     * @param p
+     * @param i
+     * @param d
+     * @param l
+     */
+    void    SetVPID(int,int,int,int);
+
+    /**
+     * Sets DPID
+     * @param p
+     * @param i
+     * @param d
+     * @param a
+     */
+    void    SetDPID(int,int,int,int);
+
+    /**
+     * Sets left motor distance and speed
+     * @param inches Distance in ticks
+     * @param inPsec Motor Speed
+     */
+    void    DiGoLeft(int inches,int inPsec);
+
+    /**
+     * Sets right motor distance and speed
+     * @param inches Distance in ticks
+     * @param inPsec Motor Speed
+     */
+    void    DiGoRight(int inches,int inPsec);
+
+    /**
+     * Sets both motors distance and speed
+     * @param inches Distance in ticks
+     * @param inPsec Motor Speed
+     */
+    void    DiGo(int inches,int inPsec);
+
+    void    SetLeftPWM(int pwm);
+    void    SetRightPWM(int pwm);
+    void    SetPWM(int pwm);
+    void    SetPWM(int lPwm, int rPwm);
+    int     IsBusy();
+
+    /**
+     * Stops both motors
+     */
+    void    Stop();
+
+    void    TurnLeft(int deg);
+    void    TurnRight(int deg);
+    void    PivetLeft(int deg);
+    void    PivetRight(int deg);
+
+    int     GetCountLeft();
+    int     GetCountRight();
+    int     GetCount();
+    float   GetDistanceLeft();
+    float   GetDistanceRight();
+    float   GetDistance();
+
+
+
+
+
+//protected:
+    int     GetReply();
+    void    InterruptHandler();
+    int     Initialize();
+//private:
+    Serial          *serial;
+    volatile int    leftSpeed, rightSpeed;
+    volatile int    _lPWM, _rPWM;
+    volatile char   _isBusy;
+    volatile char   _irqIsBusy;
+};
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Fri Dec 17 18:27:33 2010 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/Servo/#36b69a7ced07

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VS1002.cpp	Fri Dec 17 18:27:33 2010 +0000
@@ -0,0 +1,282 @@
+#include "VS1002.h"
+#include "mbed.h"
+
+
+
+/* ==================================================================
+ * Constructor
+ * =================================================================*/
+VS1002::VS1002(
+PinName mmosi, PinName mmiso, PinName ssck, PinName ccs, const char *name,
+         PinName mosi, PinName miso, PinName sck, PinName cs, PinName rst,
+         PinName dreq, PinName dcs, PinName vol)
+    :
+     _sd(mmosi, mmiso, ssck, ccs, name),
+     _spi(mosi, miso, sck), 
+     _CS(cs), 
+     _RST(rst), 
+     _DREQ(dreq),
+     _DCS(dcs), 
+     _VOL(vol) {
+    
+    }    
+
+/*===================================================================
+ * Functions
+ *==================================================================*/
+ 
+void VS1002::cs_low(void)
+{
+    _CS = 0;                                
+}
+void VS1002::cs_high(void)
+{
+    _CS = 1;                                
+}
+void VS1002::dcs_low(void)
+{
+    _DCS = 0;
+}
+void VS1002::dcs_high(void)
+{
+    _DCS = 1;
+}
+void VS1002::sci_en(void)                    //SCI enable
+{
+    cs_high();
+    dcs_high();
+    cs_low();
+}
+void VS1002::sci_dis(void)                    //SCI disable
+{
+    cs_high();
+}
+void VS1002::sdi_en(void)                    //SDI enable
+{
+    dcs_high();
+    cs_high();
+    dcs_low();
+}
+void VS1002::sdi_dis(void)                    //SDI disable
+{
+    dcs_high();
+}
+void VS1002::reset(void)                    //hardware reset
+{
+    wait(0.01);
+    _RST = 0;
+    wait(0.01);
+    _RST = 1;
+    wait(0.10);
+}
+void VS1002::power_down(void)                //hardware and software reset
+{
+    cs_low();
+    reset();
+    sci_write(0x00, SM_PDOWN);
+    wait(0.01);
+    reset();
+}
+void VS1002::sci_initialise(void)
+{
+    _RST = 1;                                //no reset
+    _spi.format(8,0);                        //spi 8bit interface, steady state low
+    _spi.frequency(1000000);                //rising edge data record, freq. 1Mhz
+    
+    cs_low();
+    for(int i=0; i<4; i++)
+    {
+    _spi.write(0xFF);                        //clock the chip a bit
+    }
+    cs_high();
+    dcs_high();
+    wait_us(5);
+ //   fprintf(stderr, "Timeout waiting for v1.x card\n");
+
+}
+void VS1002::sdi_initialise(void)
+{
+    _spi.format(8,0);
+    _spi.frequency(7000000);                //set to 7MHz
+    
+    cs_high();
+    dcs_high();
+}
+void VS1002::sci_write(unsigned char address, unsigned short int data)
+{
+    sci_en();                                //enables SCI/disables SDI
+    
+    while(!_DREQ);                            //wait unitl data request is high
+    _spi.write(0x02);                        //SCI write
+    _spi.write(address);                    //register address
+    _spi.write((data >> 8) & 0xFF);            //write out first half of data word
+    _spi.write(data & 0xFF);                //write out second half of data word
+    
+    sci_dis();                                //enables SDI/disables SCI
+    wait_us(5);
+}
+void VS1002::sdi_write(unsigned char datum)
+{
+    sdi_en();
+    
+    while(!_DREQ);
+    _spi.write(datum);
+    
+    sci_dis();
+}
+unsigned short int VS1002::read(unsigned short int address)
+{
+    cs_low();                                //enables SCI/disables SDI
+    
+    while(!_DREQ);                            //wait unitl data request is high
+    _spi.write(0x03);                        //SCI write
+    _spi.write(address);                    //register address
+    unsigned short int received = _spi.write(0x00);    //write out dummy byte
+    received <<= 8;
+    received += _spi.write(0x00);            //write out dummy byte
+    
+    cs_high();                                //enables SDI/disables SCI
+    
+    return received;                        //return received word
+}
+void VS1002::sine_test_activate(unsigned char wave)
+{
+    cs_high();                                //enables SDI/disables SCI
+    
+    while(!_DREQ);                            //wait unitl data request is high
+    _spi.write(0x53);                        //SDI write
+    _spi.write(0xEF);                        //SDI write
+    _spi.write(0x6E);                        //SDI write
+    _spi.write(wave);                        //SDI write
+    _spi.write(0x00);                        //filler byte
+    _spi.write(0x00);                        //filler byte
+    _spi.write(0x00);                        //filler byte
+    _spi.write(0x00);                        //filler byte
+
+    cs_low();                                //enables SCI/disables SDI
+}
+void VS1002::sine_test_deactivate(void)
+{
+    cs_high();
+    
+    while(!_DREQ);
+    _spi.write(0x45);                        //SDI write
+    _spi.write(0x78);                        //SDI write
+    _spi.write(0x69);                        //SDI write
+    _spi.write(0x74);                        //SDI write
+    _spi.write(0x00);                        //filler byte
+    _spi.write(0x00);                        //filler byte
+    _spi.write(0x00);                        //filler byte
+    _spi.write(0x00);                        //filler byte
+}
+void VS1002::volume(void)
+{
+ #ifdef FIXED_VOL
+    unsigned char volumize = (0 * 255); // FIXED VOL (not support volume input)
+ #else
+    unsigned char volumize = (0 * 255);
+ #endif
+    while(!_DREQ);
+    
+    unsigned short int attenuation = ((256 * volumize) + volumize);
+    sci_write(0x0B, attenuation);
+}
+
+void VS1002::play_song(int song_number)
+{
+    /*====== Song Select ======*/
+    
+//    char list[10000] = {0};
+    char list[1000] = {0};
+    char str[16] = {"/sd/"};
+    unsigned int startplace = 0;
+    unsigned int endplace = 0;
+    unsigned int play = 0;
+    num_of_files = 0;
+
+    DIR *d;
+    struct dirent *p;
+    d = opendir("/sd");
+    if(d != NULL) 
+           {
+               while((p = readdir(d)) != NULL) 
+           {
+               strcat(list, "*");
+               strcat(list, p->d_name);
+               fprintf(stderr,"\n\rFilename:%s", p->d_name);
+               num_of_files++;
+           }
+    }
+    else 
+    {
+         perror("Could not open directory!");
+    }
+    strcat(list, "*");                                //terminating *
+    if(num_of_files < song_number)
+    {
+        return;
+    }
+    while(play != song_number)
+    {
+        char symbol = list[startplace];
+        startplace++;
+        if(symbol == 0x2A)                        //0x2A = "*"    
+        {
+            play++;
+        }                        
+    }
+    play = 0;
+    while(play != (song_number+1))
+    {
+        char symbol = list[endplace];
+        endplace++;    
+        if(symbol == 0x2A)                        //0x2A = "*"    
+        {
+            play++;
+        }
+    }
+
+    strncat(str, &list[startplace], endplace-startplace);
+    str[(endplace-startplace)+3] = '\0';
+
+//printf("list: %s\r\n",list); //debug      
+
+    /*====== File Transfer ======*/
+
+     // return if not MP3 file
+    if (!((strstr(str,"MP3")!=NULL)||(strstr(str,"mp3")!=NULL))) return;
+    // display filename.mp3
+    fprintf(stderr, "\n\rNow Playing: %s\r\n",str);
+    
+    FILE *song;
+    unsigned char array[512];
+        
+    song = fopen(str, "rb");
+    
+    if(!song) 
+    {
+        exit(1);
+    }
+   
+       while(!feof(song))
+    {
+           fread(&array, 1, 512, song);
+           for(int i=0; i<512; i++)
+           {
+#ifndef FS_ONLY
+            sdi_write(array[i]);
+        //    printf(".");
+#endif
+           }
+#ifndef FS_ONLY
+            volume();
+#endif
+       }
+    for(int n=0; n<2048; n++)
+       {
+#ifndef FS_ONLY
+           sdi_write(0x00);
+#endif
+       }
+    fclose(song);                                //close the file
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VS1002.h	Fri Dec 17 18:27:33 2010 +0000
@@ -0,0 +1,74 @@
+#ifndef VS1002_H
+#define VS1002_H
+
+//#define FS_ONLY
+#define FIXED_VOL
+
+#include "mbed.h"
+#include "SDFileSystem.h"
+#include "string"
+#include "string.h"
+
+//SCI_MODE register bits as of p.26 of the datasheet
+#define SM_DIFF         0x0001
+#define SM_SETTOZERO    0x0002
+#define SM_RESET        0x0004
+#define SM_OUTOFWAV     0x0008
+#define SM_PDOWN        0x0010
+#define SM_TESTS        0x0020
+#define SM_STREAM       0x0040
+#define SM_PLUSV        0x0080
+#define SM_DACT         0x0100
+#define SM_SDIORD       0x0200
+#define SM_SDISHARE     0x0400
+#define SM_SDINEW       0x0800
+#define SM_ADPCM        0x1000
+#define SM_ADPCM_HP     0x2000
+
+
+class VS1002  {
+
+public:
+//    VS1002(int _mmosi, int _mmiso, int _ssck, int _ccs, const char* _name,
+// int _mosi, int _miso, int _sck, int _cs, int _rst, int _dreq, int _dcs, int _vol);
+    VS1002(PinName _mmosi, PinName _mmiso, PinName _ssck, PinName _ccs, const char* _name, 
+    PinName _mosi, PinName _miso, PinName _sck, PinName _cs, PinName _rst, PinName _dreq,
+    PinName _dcs, PinName _vol);
+    
+    void cs_low(void); 
+    void cs_high(void);
+    void dcs_low(void);
+    void dcs_high(void);
+    void sci_en(void);
+    void sci_dis(void);
+    void sdi_en(void);
+    void sdi_dis(void);
+    
+    void sci_initialise(void);
+    void sdi_initialise(void);
+    void reset(void);
+    void power_down(void);
+ 
+    void sci_write(unsigned char, unsigned short int);
+    void sdi_write(unsigned char);
+    unsigned short int read(unsigned short int);
+    void sine_test_activate(unsigned char);
+    void volume(void);
+    void sine_test_deactivate(void);
+    void play_song(int);
+    
+    int num_of_files;
+    
+    DigitalIn _DREQ;
+    DigitalOut _RST;
+    AnalogIn _VOL;
+    
+protected:
+
+    SPI _spi;
+    DigitalOut _CS;
+    DigitalOut _DCS;
+    SDFileSystem _sd;
+
+};
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Dec 17 18:27:33 2010 +0000
@@ -0,0 +1,321 @@
+#include "mbed.h"
+#include "Servo.h"
+#include "SRF05.h"
+#include "VS1002.h"
+#include "Serializer.h"
+#include "CMPS03.h"
+#include "math.h"
+
+SRF05 srf1(p22, p23);
+SRF05 srf2(p24, p25);
+Serial pc(USBTX, USBRX);
+Servo myservo(p21);
+VS1002 mp3(p5, p6, p7, p8, "sd",
+        p11, p12, p13, p14, p15,
+        p16, p17, p20);     //p14 in BoB non-functional so replace with p18
+Serializer robot;
+//CMPS03 compass(p9,p10, CMPS03_DEFAULT_I2C_ADDRESS);
+
+
+float motor_turn();
+void deviate_right(float angle);
+void deviate_left(float angle);
+int free_space_found = 0;
+int counter =1;
+
+int main()
+{
+   
+    float angle = 0;
+    float distance1 = 200;
+    float distance2 = 0;
+    int obstacle = 0;
+    int obstacle_count = 0;
+    myservo.calibrate(.0009, 180);
+   
+    #ifndef FS_ONLY   
+    mp3._RST = 1;
+    mp3.cs_high();                                   //chip disabled
+    mp3.sci_initialise();
+    pc.printf( "\n\rInitializing SCI!");
+                           //initialise MBED
+    mp3.sci_write(0x00,(SM_SDINEW+SM_STREAM+SM_DIFF));
+    pc.printf( "\n\rWrite1!");
+
+    mp3.sci_write(0x03, 0x9800);
+    pc.printf( "\n\rWrite2!");
+
+    mp3.sdi_initialise();   
+    pc.printf("\n\rInitialing SDI!");
+
+#endif        
+    while(1)   
+    {
+        pc.printf("\n\rNew code!!!");
+        myservo = 0.5;
+        wait(1);
+        //distance1 = srf1.read();
+      //  distance2 = 200;
+      distance1 = 200;
+        distance2 = srf2.read();
+        pc.printf("\n\rDistance1:%f", distance1);
+        pc.printf("\n\rDistance2:%f", distance2);
+        if(distance1 > 100 && distance2 > 120 )
+        {
+            pc.printf("\n\r No obstacle! Go straight");
+            obstacle = 0;
+            robot.SetSpeed(70);
+           
+        }
+        else
+        {
+            obstacle = 1;
+            obstacle_count++;   
+           // robot.Stop();
+        }
+       
+        if(obstacle_count == 1)
+        {
+            obstacle_count = 0;
+            robot.Stop();
+            mp3.play_song(11);
+            angle = motor_turn();
+            if( angle == -1000 )
+            {
+                pc.printf("\n\rStuck!");
+              //  robot.SetSpeed(-70);
+              //go back
+               
+            }
+            else if (angle > 0)
+            {
+                pc.printf("\n\rCalling deviate_right(%f)", angle);
+                deviate_right(angle);
+                robot.SetSpeed(70);
+              // robot.PivetRight(angle);
+            }
+            else if(angle < 0 )
+            {
+                pc.printf("\n\rCalling deviate_left(%f)", angle);
+                deviate_left(angle);
+                robot.SetSpeed(70);
+             //robot.PivetLeft(abs (angle));
+             }
+             else if (angle == 0)
+             { 
+                 pc.printf("\n\rContine going straight!");        
+                 robot.SetSpeed(70);
+             }
+        }
+      
+    }
+}
+
+
+float motor_turn()
+{
+   free_space_found = 0;
+   counter = 1;
+    float distance1 = 0;
+    float distance2 = 0;
+    for(float i = 0.5; i <= 1; i=i+0.16)
+    {
+        myservo = i;
+        wait(1);
+     //   distance1 = srf1.read();
+        distance2 = srf2.read();
+        pc.printf("\n\rDistance1:%f", distance1);
+        pc.printf("\n\rDistance2:%f", distance2);
+        pc.printf("\n\rAngle:%f" , (90+180*(i-0.5)));
+        pc.printf("\n\ri:%f", i);
+       
+        if( distance1 > 100 && distance2 > 120)
+        {
+            pc.printf("\n\rFree space found at %f degree!", (90+180*(i-0.5)));
+            free_space_found = 1;
+            mp3.play_song(counter);
+            return (180*(i-0.5));
+          
+        }
+        counter++;
+    }
+    if( free_space_found == 0 )
+    {
+        pc.printf("\n\rNo free space on right side!!");
+         for(float i = 0.5; i >= 0 ; i=i-0.16)
+         {
+            myservo = i;
+            wait(1);
+          //  distance1 = srf1.read();
+            distance2 = srf2.read();
+            pc.printf("\n\rDistance1:%f", distance1);
+            pc.printf("\n\rDistance2:%f", distance2);
+              pc.printf("\n\ri:%f", i);
+            pc.printf("\n\rAngle:%f" , (90+180*(i-0.5)));
+            if( distance1 > 100 && distance2 > 120 )
+            {
+               pc.printf("\n\rFree space found at %f degree!", (90+180*(i-0.5)));
+               free_space_found = 1;
+                mp3.play_song(counter);
+               return (180*(i-0.5));
+              
+            }
+            counter++;
+         }
+    }
+   
+    if( free_space_found == 0)
+    {
+        pc.printf("\n\rNo free space found! Go back!");
+        mp3.play_song(10);
+        return(-1000);
+    }
+    return 0.0;
+}
+
+
+void deviate_right(float angle)
+{
+    float distance1 = 0;
+    float distance2 = 0;
+    float distance3 = 0;
+    float distance4 =0;
+    float i = 0.5;
+    float deviation = 0;//Check for float with pavel
+    int count_deviation = 0;
+    pc.printf("\n\rInside deviate_right()");
+    robot.PivetRight(angle);
+    wait(2);
+    pc.printf("\n\rPivetRight(%f)",angle);
+    robot.ClearCount();
+    pc.printf("\n\rClear count!");
+    robot.SetSpeed(70);
+    pc.printf("\n\rSetSpeed(70)");
+    while(1)
+    {
+        pc.printf("\n\rInside while!!!");
+        count_deviation = 0;
+        myservo = 0.5;
+        wait(1);
+        //distance1 = srf1.read();
+        distance2 = srf2.read();
+        i = (-1*angle/180 + 0.5);
+        myservo = i;
+        wait(1);
+       // distance3 = srf1.read();
+       distance4 = srf2.read();
+       
+        if(distance3 > 100 && distance4 > 120)
+        {
+            pc.printf("\n\rFree space at 2*i");
+            deviation = robot.GetDistance();
+            count_deviation = robot.GetCount();
+            pc.printf("\n\rDeviation: %f, Count_deviation:%d", deviation, count_deviation);
+            robot.Stop();
+            mp3.play_song(11);
+            pc.printf("\n\rStop!");
+            robot.PivetLeft(angle);
+            mp3.play_song(counter+4);
+            wait(2);
+            robot.SetSpeed(70);
+            pc.printf("\n\rPivetleft(2*%f)", angle);
+        //    robot.DiGo(count_deviation, 70); //assuming that there is no obstacle in return path!
+          //  pc.printf("\n\rDiGo(%d,70)", count_deviation);
+          /*  while(count_deviation != 0 )
+            {
+                pc.printf("\n\rcount_deviation:%d", count_deviation);
+                count_deviation --;
+                wait(0.01);
+            }*/
+           // wait(5);   
+           // robot.Stop();//??
+           // pc.printf("\n\rStop!");
+           // robot.PivetRight(angle);
+           // pc.printf("\n\rPivetRight(%f)", angle);
+            return;      
+        }
+        if( distance1 < 100 || distance2 < 120 ) //changed!
+        {
+            pc.printf("\n\rStuck!");
+            robot.Stop();
+             mp3.play_song(11);
+            //goback
+        }
+           
+    }
+}
+
+
+void deviate_left(float angle)
+{
+    float distance1 = 0;
+    float distance2 = 0;
+    float distance3 = 0;
+    float distance4 = 0;
+    float i = 0.5;
+    float deviation = 0;//Check for float with pavel
+    int count_deviation = 0;
+    robot.PivetLeft(-angle);
+    wait(2);
+    pc.printf("\n\rInside deviate_left!!");
+    pc.printf("\n\rPivetLeft(%f)", -angle);
+    robot.ClearCount();
+    pc.printf("\n\rClearCount");
+    robot.SetSpeed(70);
+    pc.printf("\n\rSetSpeed(70)");
+    while(1)
+    {
+        count_deviation = 0;
+        myservo = 0.5;
+        wait(1);
+      //  distance1 = srf1.read();
+        distance2 = srf2.read();
+        i = (-1*angle/180 + 0.5);
+        myservo = i;
+        wait(1);
+       // distance3 = srf1.read();
+        distance4 = srf2.read();
+        if(distance3 > 100 && distance4 > 120)
+        {
+            pc.printf("\n\r Free space at 2*i!");
+            deviation = robot.GetDistance();
+            count_deviation = robot.GetCount();
+            pc.printf("\n\rDeviation:%f, count_deviation:%d", deviation, count_deviation);
+            robot.Stop();
+             mp3.play_song(11);
+            pc.printf("\n\rStop!");
+            robot.PivetRight(-1*angle);
+             mp3.play_song(counter-4);
+            wait(2);
+            pc.printf("\n\rPivetRight(2*%f)", -angle);
+            robot.SetSpeed(70);
+          //  robot.DiGo(count_deviation, 70); //assuming that there is no obstacle in return path!
+         //   pc.printf("\n\rDigo(%d,70)", count_deviation);
+         /*   while(count_deviation != 0 )
+            {
+                pc.printf("\n\rcount_deviation:%d",count_deviation);
+                count_deviation --;
+                wait(0.01);
+            }*/
+         //   wait(5);
+          //  robot.Stop();//??
+          ////  pc.printf("\n\rStop!");
+          //  robot.PivetLeft(-angle);
+          //  wait(5);
+          //  pc.printf("\n\rPivetLeft(%f)", -angle);
+            return;      
+        }
+        if( distance1 < 100 || distance2 < 120 ) //changed
+        {
+            pc.printf("\n\rStuck!");
+            robot.Stop();
+             mp3.play_song(11);
+            float ang = 0;
+           // ang = motor_turn();
+           
+           
+            //goback
+        }    
+    }
+
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Dec 17 18:27:33 2010 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/e2ac27c8e93e