final cup

Dependencies:   HMC6352 PID mbed

Files at this revision

API Documentation at this revision

Comitter:
akudohune
Date:
Mon Apr 29 06:39:53 2013 +0000
Commit message:
final

Changed in this revision

HMC6352.lib Show annotated file Show diff for this revision Revisions of this file
PID.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
main.h Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
uart1.cpp Show annotated file Show diff for this revision Revisions of this file
uart1.h Show annotated file Show diff for this revision Revisions of this file
wordString.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 2eebd0301c66 HMC6352.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC6352.lib	Mon Apr 29 06:39:53 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/HMC6352/#83c0cb554099
diff -r 000000000000 -r 2eebd0301c66 PID.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Mon Apr 29 06:39:53 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
diff -r 000000000000 -r 2eebd0301c66 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 29 06:39:53 2013 +0000
@@ -0,0 +1,586 @@
+#include <math.h>
+#include <sstream>
+#include "mbed.h"
+#include "HMC6352.h"
+#include "PID.h"
+#include "main.h"
+
+
+
+void PidUpdate()
+{   
+    static uint8_t Fflag = 0;
+    
+    inputPID = (((int)(compass.sample() - ((standard + standTu) * 10.0) + 5400.0) % 3600) / 10.0);        
+    
+    //pc.printf("%f\n",timer1.read());
+    pid.setProcessValue(inputPID);
+    //timer1.reset();
+    
+    compassPID = -(pid.compute());
+    
+    if(!Fflag){
+        Fflag = 1;
+        compassPID = 0;
+    }
+    //pc.printf("%f\n",standard);
+    //pc.printf("%d\n",diff);
+    //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
+    //pc.printf("%d\n",(int)compassPID);
+    //pc.printf("%d\t%d\n",Distance,direction);
+    //pc.printf("%d\t%d\t%d\t%d\n",ultrasonicVal[0],ultrasonicVal[1],ultrasonicVal[2],ultrasonicVal[3]);
+}
+
+void move(int vxx, int vyy, int vss)
+{
+    double motVal[MOT_NUM] = {0};
+    
+    motVal[0] = (double)(((0.5  * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1);
+    motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT2);
+    motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3);
+    motVal[3] = (double)(((0.5  * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT4);
+    
+    for(uint8_t i = 0 ; i < MOT_NUM ; i++){
+        if(motVal[i] > MAX_POW)motVal[i] = MAX_POW;
+        else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
+        speed[i] = (int)motVal[i];
+    }
+}
+
+/***********  Serial interrupt  ***********/
+
+void Tx_interrupt()
+{
+    array(speed[0],speed[1],speed[2],speed[3]);
+    driver.printf("%s",StringFIN.c_str());
+    //pc.printf("%s",StringFIN.c_str());
+}
+/*
+void Rx_interrupt()
+{
+    if(driver.readable()){
+        //pc.printf("%d\n",driver.getc());
+    }
+}*/
+
+
+/***********  Serial interrupt **********/
+
+
+void init()
+{
+    int scanfSuccess;
+    int printfSuccess;
+    int closeSuccess;
+    int close2Success;
+    FILE *fp;
+    
+    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    
+    StartButton.mode(PullUp);
+    CalibEnterButton.mode(PullUp);
+    CalibExitButton.mode(PullUp);
+    EEPROMButton.mode(PullUp);
+    
+    driver.baud(BAUD_RATE);
+    device2.baud(BAUD_RATE2);
+    wait_ms(MOTDRIVER_WAIT);///////////////////////////////////////////////////////////////
+    driver.printf("1F0002F0003F0004F000\r\n"); 
+    device2.printf("START");
+    
+    driver.attach(&Tx_interrupt, Serial::TxIrq);
+    //driver.attach(&Rx_interrupt, Serial::RxIrq);
+    device2.attach(&dev_rx,Serial::RxIrq);
+    device2.attach(&dev_tx,Serial::TxIrq);
+    
+    mbedleds = 1;
+    
+    while(StartButton){
+        if(!CalibEnterButton){
+            mbedleds = 2;
+            compass.setCalibrationMode(ENTER);
+            while(CalibExitButton);
+            compass.setCalibrationMode(EXIT);
+            wait(BUT_WAIT);
+            mbedleds = 4;
+         }
+         
+         if(!EEPROMButton){
+            fp = fopen("/local/out.txt", "r");
+            if(fp == NULL){
+                wait(BUT_WAIT);
+                mbedleds = 3;
+            }else{
+                scanfSuccess = fscanf(fp, "%lf",&standard);
+                if(scanfSuccess == EOF){
+                    wait(BUT_WAIT);
+                    mbedleds = 7;
+                }else{
+                    closeSuccess = fclose(fp);
+                    if(closeSuccess == EOF){
+                        wait(BUT_WAIT);
+                        mbedleds = 15;
+                    }else{
+                        wait(BUT_WAIT);
+                        mbedleds = 8;
+                    }
+                }
+             }
+         }
+         
+         if(!CalibExitButton){
+            standard = compass.sample() / 10.0;
+            
+            fp = fopen("/local/out.txt", "w");
+            if(fp == NULL){
+                wait(BUT_WAIT);
+                mbedleds = 3;
+            }else{
+                printfSuccess = fprintf(fp, "%f",standard);
+                if(printfSuccess == EOF){
+                    wait(BUT_WAIT);
+                    mbedleds = 7;
+                }else{
+                    close2Success = fclose(fp);
+                    if(close2Success == EOF){
+                        wait(BUT_WAIT);
+                        mbedleds = 15;
+                    }else{
+                        wait(BUT_WAIT);
+                        mbedleds = 4;
+                    }
+                }
+             }
+        }
+    }
+    
+    mbedleds = 0;
+    
+    pid.setInputLimits(0.0, 360.0);
+    pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
+    pid.setBias(0.0);
+    pid.setMode(AUTO_MODE);
+    pid.setSetPoint(180.0);
+    
+    pidUpdata.attach(&PidUpdate, 0.06);
+    
+    //timer1.start();
+    //Ctimer.start();
+}
+
+int main()
+{
+    int vx=0,vy=0,vs=0;
+    
+    init();
+           
+    while(1){
+        
+        hold_flag = 0;
+        vx = 0;
+        vy = 0;
+        vs = (int)compassPID;
+        
+        
+        /*************** Change state **************************/
+        
+        if(state == HOLD){
+            if(FRONT_SONIC < 100)hold_flag = 1;
+            
+            if(Distance > 140){
+                mbedleds = 1;
+                state = HOME_WAIT;
+            }else if(!((direction == 0) || (direction == 15) || (direction == 1))){
+                mbedleds = 4;
+                state = DIFFENCE;
+            }else if(!(Distance <= 30)){
+                mbedleds = 4;
+                state = DIFFENCE;
+            }
+        }else{
+            standTu = 0;
+            if(state == DIFFENCE){
+                
+                if((direction == 0) && (Distance <= 10) && (IR_found)){
+                    mbedleds = 8;
+                    state = HOLD;
+                }else if((Distance <= 140) && (IR_found)){
+                    mbedleds = 4;
+                    state = DIFFENCE;
+                }else{
+                    if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){
+                        if(IR_found){
+                            mbedleds = 4;
+                            state = DIFFENCE;
+                        }
+                    }else if(diff > 15){
+                         mbedleds = 4;
+                         state = DIFFENCE;
+                    }else{
+                        mbedleds = 1;
+                        state = HOME_WAIT;
+                    }
+                }
+                
+            }else{  
+                if((direction == 0) && (Distance <= 10) && (IR_found)){
+                    mbedleds = 8;
+                    state = HOLD;
+                }else if((Distance <= 120) && (IR_found)){
+                    mbedleds = 4;
+                    state = DIFFENCE;
+                }else{
+                    if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){
+                        if(IR_found){
+                            mbedleds = 4;
+                            state = DIFFENCE;
+                        }       
+                    }else if(diff > 15){
+                         mbedleds = 4;
+                         state = DIFFENCE;
+                    }else if((Distance <= 120) && (IR_found)){
+                        mbedleds = 2;
+                        state = WARNING;
+                    }else{
+                        mbedleds = 1;
+                        state = HOME_WAIT;
+                    }
+                }
+            }
+        }
+        /*************** Change state **************************/
+        //pc.printf("%f\t%f\t%f\t%f\n",ultrasonicVal[0],ultrasonicVal[1],ultrasonicVal[2],ultrasonicVal[3]);
+        
+        if(state == HOME_WAIT){
+            if(((RIGHT_SONIC + LEFT_SONIC) < 1050.0) && ((RIGHT_SONIC + LEFT_SONIC) > 900.0)){
+                if((LEFT_SONIC > 450.0) && (RIGHT_SONIC > 450.0)){
+                    vx = 0;
+                }else if(RIGHT_SONIC < 450.0){
+                    vx = (int)((RIGHT_SONIC - 450.0) * 0.01 - 10.0);
+                    if(vx < -15)vx = -15;
+                }else if(LEFT_SONIC < 450.0){
+                    vx = (int)((450.0 - LEFT_SONIC ) * 0.01 + 10.0);
+                    if(vx > 15)vx = 15;
+                }
+                
+                if((RIGHT_SONIC < 330.0) || (LEFT_SONIC < 330.0)){
+                      if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){
+                        vy = 0;
+                    }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){
+                        vy = 4;
+                    }else{
+                        vy = (int)(0.01 * (30.0 - BACK_SONIC) - 4);
+                        if(vy < -10)vy = -10;
+                    }
+                }else{
+                    if((BACK_SONIC > 115.0) && (BACK_SONIC < 145.0)){
+                        vy = 0;
+                    }else if((BACK_SONIC <= 115.0) || (BACK_SONIC == PING_ERROR)){
+                        vy = 4;
+                    }else{
+                        vy = (int)(0.01 * (130.0 - BACK_SONIC) - 4);
+                        if(vy < -10)vy = -10;
+                    }
+                }
+            }else if((RIGHT_SONIC + LEFT_SONIC) <= 900.0){
+                if(BACK_SONIC < 100.0){
+                    if(RIGHT_SONIC > LEFT_SONIC){
+                        vx = 10;
+                        vy = 0;
+                    }else{
+                        vx = -10;
+                        vy = 0;
+                    }
+                }else if(BACK_SONIC < 200.0){
+                     if(RIGHT_SONIC > LEFT_SONIC){
+                        vx = 10;
+                        vy = -4;
+                    }else{
+                        vx = -10;
+                        vy = -4;
+                    }
+                }else{
+                    vx = 0;
+                    vy = -10;
+                }
+            }else{
+                if(BACK_SONIC > 500.0){
+                    if(RIGHT_SONIC > LEFT_SONIC){
+                        vx = 10;
+                        vy = -5;
+                    }else{                    
+                        vx = -10;
+                        vy = -5;
+                    }
+                }
+            }
+        }else if(state == DIFFENCE){
+            if(direction == 6){
+            
+                if(BACK_SONIC < 110.0){
+                    vy = 4;
+                }else  if(BACK_SONIC > 500.0){
+                    vy = -10;
+                }else if(BACK_SONIC > 300.0){
+                    vy = -7;
+                }else if(BACK_SONIC > 140.0){
+                    vy = -5;
+                }else{
+                    vy = 0;
+                }
+                     
+                if(LEFT_SONIC < 700.0){
+                    vx = 15;
+                }else{
+                    if((RIGHT_SONIC < 150.0) || (RIGHT_SONIC == PING_ERROR)){
+                        vx = 10;
+                        vy = -5;
+                    }else{
+                        vx = 10;
+                        vy = -5;
+                    }
+                }
+                
+            }else if(direction == 10){
+                /*
+                if((RIGHT_SONIC < 330.0) || (LEFT_SONIC < 330.0)){
+                      if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){
+                        vy = 0;
+                    }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){
+                        vy = 4;
+                    }else{
+                        vy = (int)(0.01 * (30.0 - BACK_SONIC) - 4);
+                        if(vy < -10)vy = -10;
+                    }
+                }else{
+                    if((BACK_SONIC > 115.0) && (BACK_SONIC < 145.0)){
+                        vy = 0;
+                    }else if((BACK_SONIC <= 115.0) || (BACK_SONIC == PING_ERROR)){
+                        vy = 4;
+                    }else{
+                        vy = (int)(0.01 * (130.0 - BACK_SONIC) - 4);
+                        if(vy < -10)vy = -10;
+                    }
+                }
+                     
+                if(RIGHT_SONIC < 700.0){
+                    vx = -15;
+                }else{
+                    if((LEFT_SONIC < 150.0) || (LEFT_SONIC == PING_ERROR)){
+                        vx = -10;
+                        vy = -5;
+                    }else{
+                        vx = -10;
+                        vy = -5;
+                    }
+                }
+                */
+                if(BACK_SONIC < 110.0){
+                    vy = 4;
+                }else  if(BACK_SONIC > 500.0){
+                    vy = -10;
+                }else if(BACK_SONIC > 300.0){
+                    vy = -7;
+                }else if(BACK_SONIC > 140.0){
+                    vy = -5;
+                }else{
+                    vy = 0;
+                }
+                     
+                if(RIGHT_SONIC < 700.0){
+                    vx = -15;
+                }else{
+                    if((LEFT_SONIC < 150.0) || (LEFT_SONIC == PING_ERROR)){
+                        vx = -10;
+                        vy = -5;
+                    }else{
+                        vx = -10;
+                        vy = -5;
+                    }
+                }
+            }else if(direction == 4){
+            
+                if(BACK_SONIC < 110.0){
+                    vy = 4;
+                }else  if(BACK_SONIC > 500.0){
+                    vy = -10;
+                }else if(BACK_SONIC > 300.0){
+                    vy = -7;
+                }else if(BACK_SONIC > 140.0){
+                    vy = -5;
+                }else{
+                    vy = 0;
+                }
+                     
+                if(LEFT_SONIC < 700.0){
+                    vx = 15;
+                }else{
+                    if((RIGHT_SONIC < 150.0) || (RIGHT_SONIC == PING_ERROR)){
+                        vx = 10;
+                        vy = -5;
+                    }else{
+                        vx = 10;
+                        vy = -5;
+                    }
+                }
+                
+            }else if(direction == 12){
+            
+                if(BACK_SONIC < 110.0){
+                    vy = 4;
+                }else  if(BACK_SONIC > 500.0){
+                    vy = -10;
+                }else if(BACK_SONIC > 300.0){
+                    vy = -7;
+                }else if(BACK_SONIC > 140.0){
+                    vy = -5;
+                }else{
+                    vy = 0;
+                }
+                     
+                if(RIGHT_SONIC < 700.0){
+                    vx = -15;
+                }else{
+                    if((LEFT_SONIC < 150.0) || (LEFT_SONIC == PING_ERROR)){
+                        vx = -10;
+                        vy = -5;
+                    }else{
+                        vx = -10;
+                        vy = -5;
+                    }
+                }
+                
+            }else if(direction == 8){
+            
+                if(LEFT_SONIC > RIGHT_SONIC){
+                    vx = -10;
+                }else{
+                    vx = 10;
+                }
+                vy = -5;
+                
+            }else if(direction == 2){
+                /*
+                if((RIGHT_SONIC > 700.0) && (!(RIGHT_SONIC == PING_ERROR))){
+                    vx = 30;
+                }else{
+                    vx = 20;
+                } 
+                */
+                vx = 15;
+                if(BACK_SONIC < 130.0){
+                    //vy = 5;
+                }
+                /*
+                if(BACK_SONIC > 140.0){
+                    vy = -10;
+                }else if(BACK_SONIC < 110.0){
+                    vy = 5;
+                }
+                */
+            }else if(direction == 14){
+                /*
+                if((LEFT_SONIC > 700.0) && (!(LEFT_SONIC == PING_ERROR))){
+                    vx = -30;
+                }else{
+                    vx = -20;
+                } 
+                */
+                vx = -15;   
+                
+                if(BACK_SONIC < 130.0){
+                    //vy = 5;
+                }
+                
+                /*
+                if(BACK_SONIC > 140.0){
+                    vy = -10;
+                }else if(BACK_SONIC < 110.0){
+                    vy = 5;
+                }
+                */
+            }else if(direction == 1){
+            
+                vx = 10;    
+                
+            }else if(direction == 15){
+            
+                vx = -10;
+                
+            }else if(direction == 0){
+            
+                vx = 0;
+                vy = 10;
+                
+            }else{//error
+            
+                vx = 0;
+                vy = 0;
+            
+            }
+        }else if(state == WARNING){
+            if(direction == 0){
+            
+                vx = 0;
+                
+            }else if(direction == 1){
+            
+                vx = 15;
+                
+            }else if(direction == 2){
+            
+                vx = 20;
+                
+            }else if(direction == 14){
+            
+                vx = -20;
+                
+            }else if(direction == 15){
+            
+                vx = -15;
+                
+            }
+            
+            if((LEFT_SONIC > 320) && (RIGHT_SONIC > 320)){
+                if(BACK_SONIC > 150.0){
+                    vy = -5;
+                }else if(BACK_SONIC < 110.0){
+                    vy = 5;
+                }else{
+                    vy = 0;
+                }
+            }else{
+                if(BACK_SONIC > 110.0){
+                    vy = -5;
+                }else if(BACK_SONIC < 70.0){
+                    vy = 5;
+                }else{
+                    vy = 0;
+                }
+            }
+        }else if(state == HOLD){
+            vy = 10;
+            if(FRONT_SONIC < 100){
+                if(RIGHT_SONIC < LEFT_SONIC){
+                    if(BACK_SONIC > 500){
+                        vy = 0;
+                        vs = 3;
+                    }
+                }else{
+                    if(BACK_SONIC > 500){
+                        vy = 0;
+                        vs = -3;
+                    }
+                }
+            }else{
+                if(((RIGHT_SONIC + LEFT_SONIC) < 1050.0) && ((RIGHT_SONIC + LEFT_SONIC) > 800.0)){
+                    standTu = (RIGHT_SONIC - LEFT_SONIC) / 40.0;
+                }else{
+                    standTu = 0;
+                }
+            }
+        }
+        //vx = (int)(vx * 0.8);
+        move(vx,vy,vs);
+    }
+}
\ No newline at end of file
diff -r 000000000000 -r 2eebd0301c66 main.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Mon Apr 29 06:39:53 2013 +0000
@@ -0,0 +1,83 @@
+
+
+#define RATE    0.052
+#define Long    1.0
+#define ENTER   0
+#define EXIT    1
+#define MOT_NUM 4
+#define MOTDRIVER_WAIT  300 //ms
+#define BAUD_RATE       115200
+#define BAUD_RATE2      19200
+#define BUT_WAIT        0.3 //s
+#define ON      1
+#define OFF     0
+
+#define PING_ERROR  0xFFFF
+
+#define MOT1    1.0
+#define MOT2    1.0
+#define MOT3    1.0
+#define MOT4    1.0
+
+#define OUT_LIMIT   20.0
+#define MAX_POW     100
+#define MIN_POW     -100
+
+DigitalOut led1(LED1); 
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+BusOut mbedleds(LED4,LED3,LED2,LED1); 
+HMC6352 compass(p9, p10);
+Serial driver(p28, p27); // tx, rx 
+Serial device2(p13, p14);  // tx, rx
+Serial pc(USBTX, USBRX); // tx, rx 
+DigitalIn StartButton(p21);
+DigitalIn CalibEnterButton(p22);
+DigitalIn CalibExitButton(p23);
+DigitalIn EEPROMButton(p24);
+PID pid(0.60, 0.0, 0.010, RATE);      //battery is low power version 30.0 0.58 1.0 0.015    battery is high power version   30.0 0.42, 1.0, 0.013   power is low but perfect 20.0 0.45, 0.0, 0.010
+Ticker pidUpdata;
+Timer timer1;
+Timer Ctimer;
+LocalFileSystem local("local");  // マウントポイントを定義(ディレクトリパスになる)
+
+enum{
+    HOME_WAIT,
+    DIFFENCE,
+    WARNING,
+    HOLD,
+};
+
+
+double standTu = 0;
+int speed[MOT_NUM] = {0};
+uint8_t hold_flag = 0;
+uint8_t state = HOME_WAIT;
+
+float inputPID = 180.0;
+static double standard;
+float compassPID = 0.0;
+
+extern int diff;
+
+extern string StringFIN;
+
+extern uint8_t direction;
+extern uint8_t Distance;
+extern uint8_t IR_found;
+
+extern void PidUpdata(void);
+extern void array(int,int,int,int);
+
+extern void dev_rx(void);
+extern void dev_tx(void);
+
+extern uint16_t ultrasonicVal[4];
+
+#define FRONT_SONIC ultrasonicVal[0]
+#define BACK_SONIC  ultrasonicVal[2]
+#define RIGHT_SONIC ultrasonicVal[1]
+#define LEFT_SONIC  ultrasonicVal[3]    
+
+
diff -r 000000000000 -r 2eebd0301c66 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Apr 29 06:39:53 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/7e6c9f46b3bd
\ No newline at end of file
diff -r 000000000000 -r 2eebd0301c66 uart1.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/uart1.cpp	Mon Apr 29 06:39:53 2013 +0000
@@ -0,0 +1,97 @@
+
+#include "mbed.h"
+#include "uart1.h"
+#include "HMC6352.h"
+
+extern Serial pc;
+extern Serial device2;
+extern HMC6352 compass;
+extern BusOut mbedleds;
+extern DigitalOut led1; 
+extern DigitalOut led2;
+extern DigitalOut led3;
+extern DigitalOut led4;
+
+extern uint8_t state;
+extern uint8_t hold_flag;
+
+int diff;
+uint16_t ultrasonicVal[4];
+uint8_t direction;
+uint8_t Distance;
+uint8_t IR_found;
+uint8_t xbee;
+
+void dev_rx()
+{
+    static uint8_t count = 0;
+    static uint8_t RecData[RECEIVE_DATA_NUM];
+    static uint8_t last_data;
+    
+    
+    
+    RecData[count] = device2.getc();
+    
+    if(RecData[KEY] == KEYCODE){
+        count++;
+        
+    }else{
+        count = 0;
+    }
+    if(count >= RECEIVE_DATA_NUM){
+        if(RecData[CHECK] == CHECKCODE){
+            //mbedleds = 15;
+            
+            if((RecData[DIRECTION] <= 15) || (RecData[DIRECTION] == 200)){
+                direction = RecData[DIRECTION];
+            }
+            if(RecData[DISTANCE] <= 180){
+                Distance = RecData[DISTANCE];
+            }
+            
+            ultrasonicVal[0] = (int)((RecData[SONIC1_1] + (RecData[SONIC1_2] << 8)) / 10.0);
+            if(ultrasonicVal[0] == 6553)ultrasonicVal[0] = 0xFFFF;
+            ultrasonicVal[1] = (int)((RecData[SONIC2_1] + (RecData[SONIC2_2] << 8)) / 10.0);
+            if(ultrasonicVal[1] == 6553)ultrasonicVal[1] = 0xFFFF;
+            ultrasonicVal[2] = (int)((RecData[SONIC3_1] + (RecData[SONIC3_2] << 8)) / 10.0);
+            if(ultrasonicVal[2] == 6553)ultrasonicVal[2] = 0xFFFF;
+            ultrasonicVal[3] = (int)((RecData[SONIC4_1] + (RecData[SONIC4_2] << 8)) / 10.0);
+            if(ultrasonicVal[3] == 6553)ultrasonicVal[3] = 0xFFFF;
+            
+            if((IR_found == 0) || (IR_found == 1)){
+                IR_found = RecData[IR_FOUND];
+            }
+            xbee = RecData[XBEE];
+            
+            
+            //pc.printf("%f\t%f\t%f\t%f\n",ultrasonicVal[0],ultrasonicVal[1],ultrasonicVal[2],ultrasonicVal[3]);
+            //pc.printf("%d\n",xbee);
+            //pc.printf("%d\n",IR_found);
+            
+            diff = last_data - Distance;
+            last_data = Distance;
+        }
+        count = 0;
+    }  
+}
+
+void dev_tx()
+{
+    static uint8_t count2 = 0;
+    static uint8_t SendData[SEND_DATA_NUM];
+    
+    if(count2 >= SEND_DATA_NUM){
+        SendData[KEY2]     = KEYCODE2;
+        SendData[DATA1]    = ((int)(compass.sample())) >> 8 ;
+        SendData[DATA2]    = (int)(compass.sample());
+        SendData[DATA3]    = state;
+        SendData[DATA4]    = 1;
+        SendData[CHECK2]   = CHECKCODE2;
+        
+        count2 = 0;
+        
+    }
+    device2.putc(SendData[count2]);
+    
+    count2++;
+}
\ No newline at end of file
diff -r 000000000000 -r 2eebd0301c66 uart1.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/uart1.h	Mon Apr 29 06:39:53 2013 +0000
@@ -0,0 +1,36 @@
+
+
+
+#define RECEIVE_DATA_NUM    14
+#define SEND_DATA_NUM       6
+
+#define KEYCODE             120
+#define CHECKCODE           (RecData[DIRECTION] ^ RecData[DISTANCE] ^ RecData[SONIC1_1] ^ RecData[SONIC1_2] ^ RecData[SONIC2_1] ^ RecData[SONIC2_2] ^ RecData[SONIC3_1] ^ RecData[SONIC3_2] ^ RecData[SONIC4_1] ^ RecData[SONIC4_2] ^ RecData[IR_FOUND] ^ RecData[XBEE])
+#define KEYCODE2            35
+#define CHECKCODE2          (SendData[DATA1] ^ SendData[DATA2] ^ SendData[DATA3] ^ SendData[DATA4])
+
+
+enum{
+    KEY = 0,
+    DIRECTION,
+    DISTANCE,
+    SONIC1_1,
+    SONIC1_2,
+    SONIC2_1,
+    SONIC2_2,
+    SONIC3_1,
+    SONIC3_2,
+    SONIC4_1,
+    SONIC4_2,
+    IR_FOUND,
+    XBEE,
+    CHECK,
+};
+enum{
+    KEY2 = 0,
+    DATA1,
+    DATA2,
+    DATA3,
+    DATA4,
+    CHECK2,
+};
diff -r 000000000000 -r 2eebd0301c66 wordString.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wordString.cpp	Mon Apr 29 06:39:53 2013 +0000
@@ -0,0 +1,71 @@
+
+#include <sstream>
+#include "mbed.h"
+
+string StringFIN;
+
+using namespace std;
+
+
+//extern Serial pc; // tx, rx 
+
+string IntToString(int number)
+{
+  stringstream ss;
+  ss << number;
+  return ss.str();
+}
+
+void array(int power1,int power2,int power3,int power4)
+{
+    int input[4] = {power1,power2,power3,power4};
+    int value = 0;
+    string StringA[4] = {"0","0","0","0"};
+    
+    
+    string StringX = "0";
+    string StringY = "0";
+    string StringZ = "0";
+    string String0 = "0";
+    
+    StringFIN = "0";
+    
+    for(uint8_t i = 0 ; i < 4; i++){
+        
+        value = input[i];
+        
+        StringX =  IntToString(i+1);
+        
+        if( (value < 0) && (value >= -100) ){
+            StringY = "R";
+            value = abs(value);
+            StringZ = IntToString(value);
+        }else if( (value >= 0) && (value <= 100) ){
+            StringY = "F";
+            StringZ = IntToString(value);
+        }else{
+            value = abs(value);
+            StringY = "F";
+            StringZ = "000";
+        }
+        
+        if(value < 10){
+            String0 = "00";
+            StringZ = String0 + StringZ;
+        }else if(value < 100)
+        {
+            String0 = "0";
+            StringZ = String0 + StringZ;
+        }else{
+            
+        }
+        
+        StringA[i] = (StringX + StringY + StringZ);
+        
+        if(i == 0)StringFIN  = StringA[i];
+        else StringFIN  += StringA[i];
+        
+    }
+    
+    StringFIN += "\r\n";   
+}
\ No newline at end of file