final

Files at this revision

API Documentation at this revision

Comitter:
akudohune
Date:
Wed May 01 08:47:59 2013 +0000
Commit message:
aa

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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC6352.lib	Wed May 01 08:47:59 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/HMC6352/#83c0cb554099
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Wed May 01 08:47:59 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed May 01 08:47:59 2013 +0000
@@ -0,0 +1,576 @@
+#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) + 9000.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;
+    uint8_t MissFlag = 0;
+    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);
+    
+    pid.setInputLimits(MINIMUM,MAXIMUM);
+    pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
+    pid.setBias(PID_BIAS);
+    pid.setMode(AUTO_MODE);
+    pid.setSetPoint(REFERENCE);
+    
+    Survtimer.start();
+    
+    mbedleds = 1;
+    
+    while(StartButton){
+        MissFlag = 0;
+        if(!CalibEnterButton){
+            mbedleds = 2;
+            compass.setCalibrationMode(ENTER);
+            while(CalibExitButton);
+            compass.setCalibrationMode(EXIT);
+            wait(BUT_WAIT);
+            mbedleds = 4;
+         }
+         
+         if(!EEPROMButton){
+            Survtimer.reset();
+            fp = fopen("/local/out.txt", "r");
+            if(fp == NULL){
+                wait(BUT_WAIT);
+                MissFlag = 1;
+            }else{
+                scanfSuccess = fscanf(fp, "%lf",&standard);
+                if(scanfSuccess == EOF){
+                    wait(BUT_WAIT);
+                    MissFlag = 1;
+                }else{
+                    closeSuccess = fclose(fp);
+                    if(closeSuccess == EOF){
+                        wait(BUT_WAIT);
+                        MissFlag = 1;
+                    }else{
+                        wait(BUT_WAIT);
+                    }
+                }
+            }
+            if((Survtimer.read() > (BUT_WAIT + 0.1)) || (MissFlag)){
+                for(uint8_t i = 0;i < 2;i++){
+                    mbedleds = 15;
+                    wait(0.1);
+                    mbedleds = 0;
+                    wait(0.1);
+                }
+                mbedleds = 15;
+            }else{
+                mbedleds = 8;
+            }
+         }
+         
+         if(!CalibExitButton){
+            Survtimer.reset();
+         
+            standard = compass.sample() / 10.0;
+            
+            fp = fopen("/local/out.txt", "w");
+            if(fp == NULL){
+                wait(BUT_WAIT);
+                MissFlag = 1;
+            }else{
+                printfSuccess = fprintf(fp, "%f",standard);
+                if(printfSuccess == EOF){
+                    wait(BUT_WAIT);
+                    MissFlag = 1;
+                }else{
+                    close2Success = fclose(fp);
+                    if(close2Success == EOF){
+                        wait(BUT_WAIT);
+                        MissFlag = 1;
+                    }else{
+                        wait(BUT_WAIT);
+                    }
+                }
+            }
+            if((Survtimer.read() > (BUT_WAIT + 0.2)) || (MissFlag)){
+                for(uint8_t i = 0;i < 4;i++){
+                    mbedleds = 15;
+                    wait(0.1);
+                    mbedleds = 0;
+                    wait(0.1);
+                }
+                mbedleds = 15;
+            }else{
+                mbedleds = 10;
+            }
+        }
+    }
+    
+    mbedleds = 0;
+    
+    Survtimer.stop();
+    
+    pidUpdata.attach(&PidUpdate, PID_CYCLE);
+    
+    //timer1.start();
+}
+
+int main()
+{
+    int vx=0,vy=0,vs=0;
+    
+    init();
+           
+    while(1){
+        
+        hold_flag = 0;
+        vx = 0;
+        vy = 0;
+        vs = (int)compassPID;
+        
+        //move(vx,vy,vs);
+        
+        /******************************************************
+        *******************************************************
+        ********************Change state **********************
+        *******************************************************
+        ******************************************************/
+        
+        if(state == HOLD){
+            if(FRONT_SONIC < 100)hold_flag = 1;
+            
+            if(Distance > 140){ //審判のボール持ち上げ判定
+                state = HOME_WAIT;
+            }else if(!((direction == 0) || (direction == 15) || (direction == 1))){//ボールを見失った
+                state = DIFFENCE;
+            }else if(!(Distance <= 30)){//ボールを見失った
+                state = DIFFENCE;
+            }
+        }else{
+            standTu = 0;
+            if(state == DIFFENCE){
+                if((direction == 0) && (Distance <= 10) && (IR_found) && (!xbee)){
+                    state = HOLD;
+                }else if((Distance < 180) && (IR_found) && (!xbee)){
+                    state = DIFFENCE;
+                }else{
+                    if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){
+                        if((IR_found) && (!xbee)){
+                            state = DIFFENCE;
+                        }
+                    }else if((diff > 15) && (!xbee) && (IR_found)){
+                         state = DIFFENCE;
+                    }else{
+                        state = HOME_WAIT;
+                    }
+                }
+                
+            }else{  
+                if((direction == 0) && (Distance <= 10) && (IR_found) && (!xbee)){
+                    state = HOLD;
+                }else if((Distance <= 140) && (IR_found) && (!xbee)){
+                    state = DIFFENCE;
+                }else{
+                    if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){
+                        if((IR_found) && (!xbee)){
+                            state = DIFFENCE;
+                        }       
+                    }else if((diff > 15) && (!xbee) && (IR_found)){
+                         state = DIFFENCE;
+                    }else{
+                        state = HOME_WAIT;
+                    }
+                }
+            }
+        }
+        
+        /******************************************************
+        *******************************************************
+        ********************Change state **********************
+        *******************************************************
+        ******************************************************/
+        
+        if(state == HOME_WAIT){
+            mbedleds = 1;
+            if(((RIGHT_SONIC + LEFT_SONIC) < 1050.0) && ((RIGHT_SONIC + LEFT_SONIC) > 850.0)){
+                if((LEFT_SONIC > 425.0) && (RIGHT_SONIC > 425.0)){
+                    vx = 0;
+                }else if(RIGHT_SONIC < 425.0){
+                    vx = (int)((RIGHT_SONIC - 450.0) * 0.02 - 10.0);
+                    if(vx < -20)vx = -20;
+                }else if(LEFT_SONIC < 425.0){
+                    vx = (int)((450.0 - LEFT_SONIC ) * 0.02 + 10.0);
+                    if(vx > 20)vx = 20;
+                }
+                
+                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) <= 850.0){
+                if(BACK_SONIC < 200.0){
+                    if(RIGHT_SONIC > LEFT_SONIC){
+                        vx = 10;
+                        vy = 3;
+                    }else{
+                        vx = -10;
+                        vy = 3;
+                    }
+                }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){
+            mbedleds = 4;
+            if(direction == 6){
+                vx = 15;
+            
+                if(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.015 * (30.0 - BACK_SONIC) - 4);
+                        if(vy < -15)vy = -15;
+                    }
+                }else if(RIGHT_SONIC < 330.0){
+                    vx = 10;
+                    vy = -5;
+                }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.015 * (130.0 - BACK_SONIC) - 4);
+                        if(vy < -15)vy = -15;
+                    }
+                }
+                
+            }else if(direction == 10){
+                vx = -15;
+            
+                if(RIGHT_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.015 * (30.0 - BACK_SONIC) - 4);
+                        if(vy < -15)vy = -15;
+                    }
+                }else if(LEFT_SONIC < 330.0){
+                    vx = -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.015 * (130.0 - BACK_SONIC) - 4);
+                        if(vy < -15)vy = -15;
+                    }
+                }
+            }else if(direction == 4){
+            
+                vx = 15;
+            
+                if(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.015 * (30.0 - BACK_SONIC) - 4);
+                        if(vy < -15)vy = -15;
+                    }
+                }else if(RIGHT_SONIC < 330.0){
+                    vx = 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.015 * (130.0 - BACK_SONIC) - 4);
+                        if(vy < -15)vy = -15;
+                    }
+                }
+                
+                
+            }else if(direction == 12){
+            
+                 vx = -15;
+            
+                if(RIGHT_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.015 * (30.0 - BACK_SONIC) - 4);
+                        if(vy < -15)vy = -15;
+                    }
+                }else if(LEFT_SONIC < 330.0){
+                    vx = -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.015 * (130.0 - BACK_SONIC) - 4);
+                        if(vy < -15)vy = -15;
+                    }
+                }
+                
+            }else if(direction == 8){
+            
+                if(LEFT_SONIC > RIGHT_SONIC){
+                    vx = -15;
+                }else{
+                    vx = 15;
+                }
+                if((RIGHT_SONIC < 330.0) || (LEFT_SONIC < 330.0)){
+                    if(BACK_SONIC < 45.0){
+                        vy = -5;
+                    }else{
+                        vy = (int)(0.015 * (30.0 - BACK_SONIC) - 4);
+                        if(vy < -15)vy = -15;
+                    }
+                }else{
+                    if(BACK_SONIC < 145.0){
+                        vy = -5;
+                    }else{
+                        vy = (int)(0.015 * (130.0 - BACK_SONIC) - 4);
+                        if(vy < -15)vy = -15;
+                    }
+                }
+                
+            }else if(direction == 2){
+                
+                if(BACK_SONIC < 150.0){
+                    vx = 20;
+                    vy = 0;
+                }else{
+                    vx = 12;
+                    vy = 0;
+                }
+            }else if(direction == 14){
+                
+                if(BACK_SONIC < 150.0){
+                    vx = -20;
+                    vy = -4;
+                }else{
+                    vx = -12;
+                    vy = -2;
+                }    
+                
+            }else if(direction == 1){
+            
+                if(BACK_SONIC < 150.0){
+                    vx = 15;
+                    vy = 0;
+                }else{
+                    vx = 10;
+                    vy = 0;
+                } 
+                
+            }else if(direction == 15){
+            
+                if(BACK_SONIC < 150.0){
+                    vx = -15;
+                    vy = -3;
+                }else{
+                    vx = -10;
+                    vy = -1;
+                }
+            }else if(direction == 0){
+            
+                vx = 0;
+                vy = 10;
+                
+            }else{//error
+            
+                vx = 0;
+                vy = 0;
+            
+            }
+        }else if(state == WARNING){
+            mbedleds = 2;
+            if(direction == 0){
+            
+                vx = 0;
+                
+            }else if(direction == 1){
+            
+                vx = 10;
+                
+            }else if(direction == 2){
+            
+                vx = 15;
+                
+            }else if(direction == 14){
+            
+                vx = -15;
+                
+            }else if(direction == 15){
+            
+                vx = -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;
+                }
+            }
+        }else if(state == HOLD){
+            mbedleds = 8;
+            vy = 15;
+            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) / 30.0;
+                }
+            }
+        }
+       //move(vx,vy,vs);
+    }
+}
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Wed May 01 08:47:59 2013 +0000
@@ -0,0 +1,94 @@
+
+
+#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 PID_BIAS    0.0
+#define REFERENCE   180.0
+#define MINIMUM     0.0
+#define MAXIMUM     360.0
+
+#define PID_CYCLE   0.06    //s
+
+#define P_GAIN  0.60    
+#define I_GAIN  0.0
+#define D_GAIN  0.010
+
+#define OUT_LIMIT   30.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(P_GAIN,I_GAIN,D_GAIN, 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 Survtimer;
+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 uint8_t xbee;
+
+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]    
+
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed May 01 08:47:59 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/7e6c9f46b3bd
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/uart1.cpp	Wed May 01 08:47:59 2013 +0000
@@ -0,0 +1,94 @@
+
+#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((RecData[IR_FOUND] == 0) || (RecData[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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/uart1.h	Wed May 01 08:47:59 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,
+};
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wordString.cpp	Wed May 01 08:47:59 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