File content as of revision 3:a66f4fa1f8a8:

/**********************************************/
//  
//    
//
//  Program name: Aigamozu Robot Control
//  Author: Atsunori Maruyama
//  Ver ->  1.3
//  Day ->  2014/06/09
//  
//
/**********************************************/

/*********************/
// 2015/05/11 yokokawa mami
// AGZ_ROBOTでbase[5], robot[2], managerを宣言しました。
// XbeeAddressでbase_Address, robot_Address, manager_Addressを宣言しました。
// case STATUS_REQUEST:での条件分岐
// send_AddressのところにそのときのAddressが入るようにしました。
// case RECEIVE_STATUS:での条件分岐
// base, robot, managerに対応するように変更しました。
/*********************/

#include "mbed.h"
#include "XBee.h"
#include "MBed_Adafruit_GPS.h"
#include "AigamozuControlPackets.h"
#include "agzIDLIST.h"
#include "aigamozuSetting.h"
#include "agz_common.h"
#include "Kalman.h"

/////////////////////////////////////////
//
//Connection Setting
//
/////////////////////////////////////////

//Serial Connect Setting: PC <--> mbed
Serial pc(USBTX, USBRX);    

//Serial Connect Setting: GPS <--> mbed
Serial * gps_Serial;

//Serial Connect Setting: XBEE <--> mbed
XBee xbee(p13,p14);
ZBRxResponse zbRx = ZBRxResponse();
XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L);

AGZ_ROBOT base[5], robot[2], manager;

char MyID = 'Z';

/////////////////////////////////////////
//
//Pin Setting
//
/////////////////////////////////////////
VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);



/////////////////////////////////////////
//
//Kalman Processing
//
/////////////////////////////////////////


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]={0};

    
void get_K(){
  double temp[2][2]={
    {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
    {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
  };
  double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
  K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
  K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
}


void get_x(){
  x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
  x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][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();
}


/////////////////////////////////////////
//
//Main Processing
//
/////////////////////////////////////////
int main() {
    //start up time
    wait(3);
    //set pc frequency to 57600bps 
    pc.baud(PC_BAUD_RATE); 
    //set xbee frequency to 57600bps
    xbee.begin(XBEE_BAUD_RATE);    
    
    //GPS setting
    gps_Serial = new Serial(p28,p27);
    Adafruit_GPS myGPS(gps_Serial); 
    Timer refresh_Timer;
    const int refresh_Time = 2000; //refresh time in ms
    myGPS.begin(GPS_BAUD_RATE); 
    
    Timer collect_Timer;
    const int collect_Time = 200; //when we collect 4 GPS point, we use 
    int collect_flag = 0;
    char collect_state = 'a';
    XBeeAddress64 collect_Address[5] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 
                                        XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L),
                                        XBeeAddress64(BASE5_32H,BASE5_32L)};
    //XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
    int id, SenderIDc;
    bool flag = true;
    
    
    XBeeAddress64 base_Address[5] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 
                                    XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L),
                                    XBeeAddress64(BASE5_32H,BASE5_32L)};
    XBeeAddress64 robot_Address[2] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L)};
    XBeeAddress64 manager_Address = XBeeAddress64(BASE1_32H,BASE1_32L);
    
    XBeeAddress64 send_Address;
    
    //GPS Send Command
    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
    myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
    myGPS.sendCommand(PGCMD_ANTENNA);
    
    wait(2);
       
    //interrupt start
    AigamozuControlPackets agz(agz_motorShield);
    refresh_Timer.start();
    
    printf("start\n");
        
    
    while (true) {
        
        //Check Xbee Buffer Available
        xbee.readPacket();
            if (xbee.getResponse().isAvailable()) {
                if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
                xbee.getResponse().getZBRxResponse(zbRx);
                uint8_t *buf = zbRx.getFrameData();
                uint8_t *buf1 = &buf[11]; 
                
                //id = buf1[5] - 'A';
                SenderIDc = buf1[5];
                 
                 
                 //Check Command Type 
                 switch(agz.checkCommnadType(buf)){
                    
                    //CommandType -> ChanegeMode
                    case CHANGE_MODE :{
                        agz.changeMode(buf);                                        
                        break;
                        }

                    //CommandType -> Manual
                    case MANUAL:{  
                        //Check now Mode
                        if(agz.nowMode == MANUAL_MODE){ 
                            agz.changeSpeed(buf);
                            }
                        break;
                        }
                    
                    //CommandType -> Send Status
                    case STATUS_REQUEST:{
                            /*
                            if(SenderIDc == 'Z'){
                                //send normal data
                                //Create GPS Infomation Packet
                                agz.createReceiveStatusCommand(MyID,SenderIDc,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
                                //Select Destination
                                ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength());
                                //Send -> Base
                                xbee.send(tx64request);
                            
                                //send Kalman data
                                agz.createReceiveStatusCommandwithKalman(MyID,SenderIDc,myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
                                //Select Destination
                                ZBTxRequest tx64request1(robot_Address,agz.packetData,agz.getPacketLength());
                                //Send -> Base
                                xbee.send(tx64request1);
                                
                            }else{*/
                                
                                printf("%d: catch request\n", id);
                                
                                //send normal data
                                //Create GPS Infomation Packet
                                if(SenderIDc == '0'){
                                    send_Address = manager_Address;
                                }
                                if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
                                    send_Address = robot_Address[SenderIDc - 'A'];
                                }
                                if(SenderIDc >= 'a' && SenderIDc <= 'z'){
                                    send_Address = base_Address[SenderIDc - 'a'];
                                }
                                
                                agz.createReceiveStatusCommand(MyID,SenderIDc,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
                                //Select Destination
                                ZBTxRequest tx64request2(send_Address,agz.packetData,agz.getPacketLength());
                                //Send -> Base
                                xbee.send(tx64request2);
                            
                                //send Kalman data
                                agz.createReceiveStatusCommandwithKalman(MyID,SenderIDc,myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
                                //Select Destination
                                ZBTxRequest tx64request3(collect_Address[id],agz.packetData,agz.getPacketLength());
                                //Send -> Base
                                xbee.send(tx64request3);
                           // }
                            break;         
          
                        }
                        
                    case RECEIVE_STATUS:{
                            
                        //printf("Receive Status\n");      
    
                        if(SenderIDc == '0'){
                            id = 0;
                            manager.set_state(buf1[9]);
                            manager.set_LatitudeH(&buf1[13]);
                            manager.set_LatitudeL(&buf1[17]);
                            manager.set_LongitudeH(&buf1[21]);
                            manager.set_LongitudeL(&buf1[25]);                        
                            agz.reNewBasePoint(id,manager.get_LatitudeH(),manager.get_LatitudeL(),manager.get_LongitudeH(),manager.get_LongitudeL());
                        }        
                        if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
                            id = SenderIDc - 'A';
                            robot[id].set_state(buf1[9]);
                            robot[id].set_LatitudeH(&buf1[13]);
                            robot[id].set_LatitudeL(&buf1[17]);
                            robot[id].set_LongitudeH(&buf1[21]);
                            robot[id].set_LongitudeL(&buf1[25]);                        
                            agz.reNewBasePoint(id,robot[id].get_LatitudeH(),robot[id].get_LatitudeL(),robot[id].get_LongitudeH(),robot[id].get_LongitudeL());
                        }
                        if(SenderIDc >= 'a' && SenderIDc <= 'z'){
                            id = SenderIDc - 'a';
                            base[id].set_state(buf1[9]);
                            base[id].set_LatitudeH(&buf1[13]);
                            base[id].set_LatitudeL(&buf1[17]);
                            base[id].set_LongitudeH(&buf1[21]);
                            base[id].set_LongitudeL(&buf1[25]);                        
                            agz.reNewBasePoint(id,base[id].get_LatitudeH(),base[id].get_LatitudeL(),base[id].get_LongitudeH(),base[id].get_LongitudeL());
                        }
                    /*   
                        printf("%d,", buf1[5]);                    
                        printf(" %ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL());
                    */
                        break;
                    }
                    case RECEIVE_KALMAN:{
                        id = buf1[5] - 'A';
                        robot[id].set_state(buf1[9]);
                        robot[id].set_LatitudeKH(&buf1[13]);
                        robot[id].set_LatitudeKL(&buf1[17]);
                        robot[id].set_LongitudeKH(&buf1[21]);
                        robot[id].set_LongitudeKL(&buf1[25]);
                        
                        agz.reNewBasePointKalman(id,robot[id].get_LatitudeKH(),robot[id].get_LatitudeKL(),robot[id].get_LongitudeKH(),robot[id].get_LongitudeKL());
 
                        break;    
                    }
                    default:{
                        break;
                    }
                }
                    
            
                }
            }

            
        myGPS.read();
        //recive gps module
        //check if we recieved a new message from GPS, if so, attempt to parse it,
        if ( myGPS.newNMEAreceived() ) {
            if ( !myGPS.parse(myGPS.lastNMEA()) ) {
                continue;   
            }       
        }
        if (refresh_Timer.read_ms() >= refresh_Time) {
            refresh_Timer.reset();
            if (myGPS.fix) {
                agz.nowStatus = GPS_AVAIL;
                
                if(flag){
                    if(myGPS.longitudeH==139 && myGPS.latitudeH==37){
                        flag = false;
                        x[0][0]=(double)myGPS.longitudeL;                        
                        x[0][1]=(double)myGPS.latitudeL;   
                    } 
                }
                if(myGPS.longitudeH!=139 || myGPS.latitudeH!=37) continue;
                
                //Kalman Filter
                Kalman(myGPS.longitudeL,myGPS.latitudeL);
                //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];
                    }
                }
                
                myGPS.longitudeKH=myGPS.longitudeH;//longitude after filtering
                myGPS.latitudeKH=myGPS.latitudeH;//latitude after filtering
                myGPS.longitudeKL=(long)x[1][0];//longitude after filtering
                myGPS.latitudeKL=(long)x[1][1];//latitude after filtering
                
                agz.reNewRobotPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
                agz.reNewRobotPointKalman(myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
                //printf("state = %d\n",agz.nowMode);
                //printf("my %ld, %ld, %ld, %ld\n", myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
            }
            else agz.nowStatus = GPS_UNAVAIL;
            
        } 
        
        
        //get base GPS
        //if this program is for base,this program don't execute
        
        if( collect_Timer.read_ms() >= collect_Time){
            collect_Timer.reset();
            
            //printf("Send Request:%d,%d\n",collect_flag,collect_state);
                      
            agz.createRequestCommand(MyID, collect_state);
            //Select Destination
            ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength());
            //Send -> Base
            xbee.send(tx64request);
            
            collect_flag++;
            collect_state++;

            if(collect_flag == 4){
                collect_state = 'a';
                collect_flag = 0;
            }    
         } 
         
    } 
}