Revision 0:25715989dbc2, committed 2015-04-08

Comitter:

kityann

Date:

Wed Apr 08 13:16:59 2015 +0000

Commit message:

get GPS;

Changed in this revision

diff -r 000000000000 -r 25715989dbc2 AigamozuControlPackets.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AigamozuControlPackets.lib	Wed Apr 08 13:16:59 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/aigamozu/code/AigamozuControlPackets/#282dad679fca

diff -r 000000000000 -r 25715989dbc2 Kalmanprog.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalmanprog.cpp	Wed Apr 08 13:16:59 2015 +0000
@@ -0,0 +1,222 @@
+/*
+#include "mbed.h"
+#include "XBee.h"
+
+#include "agz_common.h"
+#include "AigamozuControlPackets.h"
+
+
+#define MY_BASE_NUMBER 0
+
+XBee xbee(p13,p14);
+Serial pc(USBTX, USBRX); // tx, rx
+
+Ticker timer;
+
+//robot data
+AGZ_ROBOT robot[AGZ_NUM_ROBOTS];
+
+
+    double sigmaGPS[2][2] = {{250,0},{0,250}};
+    double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
+    double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
+    double y[2],x[2][2];
+
+
+//function
+//transform packetdata to robot data
+void agz_transform_papcket(uint8_t* packet_data,int id){
+    //set latitude, longitude, state
+    robot[id].set_state(packet_data[9]);
+    robot[id].set_LatitudeH(&packet_data[13]);
+    robot[id].set_LatitudeL(&packet_data[17]);
+    robot[id].set_LongitudeH(&packet_data[21]);
+    robot[id].set_LongitudeL(&packet_data[25]);    
+}
+
+/*---------registere robot--------------*/
+/*
+void agz_register_robot(uint8_*t packet_data){
+    uint8_t robot_id = packet_data[5];
+    robot[robot_id].set_state(0);
+    send_register_ack();
+}
+*/
+
+/*---------timer set----*/
+/*
+void agz_send_request(){
+    static uint8_t next_send_address = 0,i;
+    AigamozuControlPackets agz_packet;
+    
+    /*------search next moving robot--------*/
+    /*
+    for(i = next_send_address;i < AGZ_NUM_ROBOTS;i++){
+        if(robot[i].get_state() != 0xFF){
+            next_send_address = i;
+            break;
+        }
+    }
+    if(i == AGZ_NUM_ROBOTS){
+        for(i = 0;i < AGZ_NUM_ROBOTS;i++){
+            if(robot[i].get_state() != 0xFF){
+                next_send_address = i;
+                break;
+            }
+        }
+    }
+    
+    agz_packet.createRequestCommand(MY_BASE_NUMBER,next_send_address);
+    
+    ZBTxRequest agz_request_packet = ZBTxRequest(Agz_XBee_Remote_Address[next_send_address], agz_packet.packetData, REQUEST_COMMNAD_LENGTH );   
+    
+    xbee.send(agz_request_packet);
+    
+    next_send_address++;
+}
+
+void get_K(){
+    double ad_bc = (sigma[0][0][0]+sigmaGPS[0][0])*(sigma[0][1][1]+sigmaGPS[1][1])-(sigma[0][1][0]+sigmaGPS[1][0])*(sigma[0][0][1]+sigmaGPS[0][1]);
+    K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(sigma[1][1][1]+sigmaGPS[1][1]);
+    K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(sigma[1][0][0]+sigmaGPS[0][0]);
+}
+
+void get_x(){
+    x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[1][0]);
+    x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[1][1]);
+}
+
+void get_sigma(){
+    double temp[2][2];
+    for(int i=0;i<2;i++) {
+        for(int j=0;j<2;j++) {
+            for(int k=0;k<2;k++) {
+                temp[i][j]+=K[1][i][k]*sigma[0][k][j];
+            }
+        }
+   }
+    for(int i = 0;i < 2;i++){
+        for(int j = 0;j < 2;j++){
+            sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
+        }
+    }   
+}
+
+void Kalman(double Latitude,double Longitude){
+    y[0] = Latitude;
+    y[1] = Longitude;
+    //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
+    get_K();
+    //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
+    get_x();
+    //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
+    get_sigma();
+}
+
+
+/*---------init---------*/
+/*
+void agz_base_init(){
+    xbee.begin(57600);
+    pc.baud(57600);
+    timer.attach(&agz_send_request,1.0);
+    
+}
+
+void Kalman_init(){
+}
+
+
+int main_Kalman(void){
+    int sender,reciever;
+
+    
+    ZBRxResponse xbeeresponse;
+    
+    agz_base_init();
+    
+    wait(1);
+    
+    printf("start\n");
+    
+    
+    //robot[0].set_state(0);
+    //robot[1].set_state(0);
+    //robot[2].set_state(0);
+    robot[3].set_state(0);
+    //robot[4].set_state(0);
+    //robot[5].set_state(0);
+    
+    int count=0;
+        
+        
+    while(1){
+        xbee.readPacket();
+
+
+
+        if(xbee.getResponse().isAvailable() ){ 
+            xbee.getResponse(xbeeresponse);
+            
+            //get data packet
+            if(xbee.getResponse().getApiId() == ZB_RX_RESPONSE){
+                uint8_t *packet_data = xbeeresponse.getData();
+                uint8_t packet_kinds = packet_data[3];
+                
+                //get sensor data
+                if(packet_kinds == 'R'){
+                    sender = packet_data[5]-1;// - 'A';
+                    reciever = packet_data[7]-1;// - 'a';
+                    agz_transform_papcket(packet_data,sender);
+                    
+                    if(count==0){
+                        count++;
+                        x[1][0]=robot[sender].get_LatitudeL();                        
+                        x[1][1]=robot[sender].get_LongitudeL();
+                    }else if(count == 100){
+                        printf("/////////////////////////Count = 100///////////////////////\n");
+                        printf("state=%d,sender=%d,reciever=%d,latH=%ld.latL=%lf,longH=%ld.longL=%lf\n",robot[sender].get_state(),sender,reciever,
+                        robot[sender].get_LatitudeH(),x[1][0],
+                        robot[sender].get_LongitudeH(),x[1][1]);
+                        for(int i = 0;i < 2;i++){
+                            for(int j = 0;j < 2;j++){
+                                printf("%lf ",sigma[0][i][j]);
+                            }
+                            printf("\n");
+                        }
+                        count = 0;          
+                    }else{
+                    count++;
+                    Kalman(robot[sender].get_LatitudeL(),robot[sender].get_LongitudeL());
+
+                    
+                    printf("%d,%d,%d,%ld.%ld,%ld.%ld\n",robot[sender].get_state(),sender,reciever,
+                    robot[sender].get_LatitudeH(),robot[sender].get_LatitudeL(),
+                    robot[sender].get_LongitudeH(),robot[sender].get_LongitudeL());
+                    
+                    printf("state=%d,sender=%d,reciever=%d,latH=%ld.latL=%lf,longH=%ld.longL=%lf\n",robot[sender].get_state(),sender,reciever,
+                    robot[sender].get_LatitudeH(),x[1][0],
+                    robot[sender].get_LongitudeH(),x[1][1]);
+                    //kousinn
+                    for(int i = 0;i < 2;i++){
+                        for(int j = 0;j < 2;j++){
+                            K[0][i][j]=K[1][i][j];
+                            x[0][i]=x[1][i];
+                            sigma[0][i][j]=sigma[1][i][j];
+                        }
+                    }
+                    }
+                }
+                //get register packet
+                if(packet_kinds == 'E'){
+                    //agz_register_robot(packet_data);
+                }
+            }else{
+                //get not rx response
+            }
+        }else{
+          
+        }
+    }
+}
+*/
\ No newline at end of file

diff -r 000000000000 -r 25715989dbc2 XBee.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/XBee.lib	Wed Apr 08 13:16:59 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/okini3939/code/XBee/#b36422ef864f

diff -r 000000000000 -r 25715989dbc2 agz_common.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/agz_common.lib	Wed Apr 08 13:16:59 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/aigamozu/code/agz_common/#d9068d64649d

diff -r 000000000000 -r 25715989dbc2 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Apr 08 13:16:59 2015 +0000
@@ -0,0 +1,224 @@
+
+#include "mbed.h"
+#include "XBee.h"
+
+#include "agz_common.h"
+#include "AigamozuControlPackets.h"
+
+
+#define MY_BASE_NUMBER 0
+
+XBee xbee(p13,p14);
+Serial pc(USBTX, USBRX); // tx, rx
+
+Ticker timer;
+
+//robot data
+AGZ_ROBOT robot[AGZ_NUM_ROBOTS];
+
+
+    double sigmaGPS[2][2] = {{250,0},{0,250}};
+    double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
+    double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
+    double y[2],x[2][2];
+
+
+//function
+//transform packetdata to robot data
+void agz_transform_papcket(uint8_t* packet_data,int id){
+    //set latitude, longitude, state
+    robot[id].set_state(packet_data[9]);
+    robot[id].set_LatitudeH(&packet_data[13]);
+    robot[id].set_LatitudeL(&packet_data[17]);
+    robot[id].set_LongitudeH(&packet_data[21]);
+    robot[id].set_LongitudeL(&packet_data[25]);    
+}
+
+/*---------registere robot--------------*/
+/*
+void agz_register_robot(uint8_*t packet_data){
+    uint8_t robot_id = packet_data[5];
+    robot[robot_id].set_state(0);
+    send_register_ack();
+}
+*/
+
+/*---------timer set----*/
+
+void agz_send_request(){
+    static uint8_t next_send_address = 0,i;
+    AigamozuControlPackets agz_packet;
+    
+    /*------search next moving robot--------*/
+    
+    for(i = next_send_address;i < AGZ_NUM_ROBOTS;i++){
+        if(robot[i].get_state() != 0xFF){
+            next_send_address = i;
+            break;
+        }
+    }
+    if(i == AGZ_NUM_ROBOTS){
+        for(i = 0;i < AGZ_NUM_ROBOTS;i++){
+            if(robot[i].get_state() != 0xFF){
+                next_send_address = i;
+                break;
+            }
+        }
+    }
+    
+    agz_packet.createRequestCommand(MY_BASE_NUMBER,next_send_address);
+    
+    ZBTxRequest agz_request_packet = ZBTxRequest(Agz_XBee_Remote_Address[next_send_address], agz_packet.packetData, REQUEST_COMMNAD_LENGTH );   
+    
+    xbee.send(agz_request_packet);
+    
+    next_send_address++;
+}
+
+void get_K(){
+    double ad_bc = (sigma[0][0][0]+sigmaGPS[0][0])*(sigma[0][1][1]+sigmaGPS[1][1])-(sigma[0][1][0]+sigmaGPS[1][0])*(sigma[0][0][1]+sigmaGPS[0][1]);
+    K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(sigma[1][1][1]+sigmaGPS[1][1]);
+    K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(sigma[1][0][0]+sigmaGPS[0][0]);
+}
+
+void get_x(){
+    x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[1][0]);
+    x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[1][1]);
+}
+
+void get_sigma(){
+    double temp[2][2];
+    for(int i=0;i<2;i++) {
+        for(int j=0;j<2;j++) {
+            for(int k=0;k<2;k++) {
+                temp[i][j]+=K[1][i][k]*sigma[0][k][j];
+            }
+        }
+   }
+    for(int i = 0;i < 2;i++){
+        for(int j = 0;j < 2;j++){
+            sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
+        }
+    }   
+}
+
+void Kalman(double Latitude,double Longitude){
+    y[0] = Latitude;
+    y[1] = Longitude;
+    //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
+    get_K();
+    //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
+    get_x();
+    //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
+    get_sigma();
+}
+
+
+/*---------init---------*/
+
+void agz_base_init(){
+    xbee.begin(57600);
+    pc.baud(57600);
+    timer.attach(&agz_send_request,1.0);
+    
+}
+
+void Kalman_init(){
+}
+
+
+int main(void){
+    int sender,reciever;
+
+    
+    ZBRxResponse xbeeresponse;
+    
+    agz_base_init();
+    
+    wait(1);
+    
+    printf("start\n");
+    
+    
+    robot[0].set_state(0);
+    //robot[1].set_state(0);
+    //robot[2].set_state(0);
+    //robot[3].set_state(0);
+    //robot[4].set_state(0);
+    //robot[5].set_state(0);
+    
+    int count=0;
+        
+        
+    while(1){
+        xbee.readPacket();
+
+
+
+        if(xbee.getResponse().isAvailable() ){ 
+            xbee.getResponse(xbeeresponse);
+            
+            //get data packet
+            if(xbee.getResponse().getApiId() == ZB_RX_RESPONSE){
+                uint8_t *packet_data = xbeeresponse.getData();
+                uint8_t packet_kinds = packet_data[3];
+                
+                //get sensor data
+                if(packet_kinds == 'R'){
+                    sender = packet_data[5]-1;// - 'A';
+                    reciever = packet_data[7]-1;// - 'a';
+                    agz_transform_papcket(packet_data,sender);
+                    
+                    
+                    if(count==0){
+                        count++;
+                        x[1][0]=robot[sender].get_LatitudeL();                        
+                        x[1][1]=robot[sender].get_LongitudeL();
+                    }else if(count == 100){
+                        printf("/////////////////////////Count = 100///////////////////////\n");
+                        printf("state=%d,sender=%d,reciever=%d,latH=%ld.latL=%lf,longH=%ld.longL=%lf\n",robot[sender].get_state(),sender,reciever,
+                        robot[sender].get_LatitudeH(),x[1][0],
+                        robot[sender].get_LongitudeH(),x[1][1]);
+                        for(int i = 0;i < 2;i++){
+                            for(int j = 0;j < 2;j++){
+                                printf("%lf ",sigma[0][i][j]);
+                            }
+                            printf("\n");
+                        }
+                        count = 0;          
+                    }else{
+                    count++;
+                    Kalman(robot[sender].get_LatitudeL(),robot[sender].get_LongitudeL());
+                    
+                    
+                    printf("%d,%d,%d,%ld.%ld,%ld.%ld\n",robot[sender].get_state(),sender,reciever,
+                    robot[sender].get_LatitudeH(),robot[sender].get_LatitudeL(),
+                    robot[sender].get_LongitudeH(),robot[sender].get_LongitudeL());
+                    
+                    
+                    printf("state=%d,sender=%d,reciever=%d,latH=%ld.latL=%lf,longH=%ld.longL=%lf\n",robot[sender].get_state(),sender,reciever,
+                    robot[sender].get_LatitudeH(),x[1][0],
+                    robot[sender].get_LongitudeH(),x[1][1]);
+                    //kousinn
+                    for(int i = 0;i < 2;i++){
+                        for(int j = 0;j < 2;j++){
+                            K[0][i][j]=K[1][i][j];
+                            x[0][i]=x[1][i];
+                            sigma[0][i][j]=sigma[1][i][j];
+                        }
+                    }
+                    }
+                    
+                }
+                //get register packet
+                if(packet_kinds == 'E'){
+                    //agz_register_robot(packet_data);
+                }
+            }else{
+                //get not rx response
+            }
+        }else{
+          
+        }
+    }
+}

diff -r 000000000000 -r 25715989dbc2 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Apr 08 13:16:59 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/024bf7f99721
\ No newline at end of file