ryo seki
final cup
Dependencies: HMC6352 PID mbed
Revision 0:2eebd0301c66, committed 2013-04-29
- Comitter:
- akudohune
- Date:
- Mon Apr 29 06:39:53 2013 +0000
- Commit message:
- final
Changed in this revision
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