File content as of revision 0:25715989dbc2:

#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{
          
        }
    }
}