File content as of revision 7:209c07cc2b80:

/**********************************************/
//  
//    
//
//  Program name: Aigamozu Robot Control
//  Author: Atsunori Maruyama
//  Ver ->  1.3
//  Day ->  2014/06/09
//  
//
/**********************************************/
//
//  中澤遥菜
//  変更点：メイン関数の中の処理を関数化しました。
//  setting(), interrupt_start(VNH5019 motorShield),
//  GPS_Setting(int refresh_Time, int collect_Time),
//  GPS_SendCommand(), check_CommandType(utf8_t command_type),
//  LatLong_afterFiltering(double longitude, double latitude, double x0, double x1),
//  send_GPSdata(), get_GPSdata(), getKalman()
//
/**********************************************/

#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 robot[5];

char Base_ID = 'A';

/////////////////////////////////////////
//
//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();
}

/////////////////////////////////////////
//
//setting
//
/////////////////////////////////////////
void setting(){
     //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);    
    }

/////////////////////////////////////////
//
//interrupt start
//
/////////////////////////////////////////
void interrupt_start(VNH5019 motorShield, Timer refresh){
    AigamozuControlPackets agz(motorShield);
    refresh.start();
    
    printf("start\n");
}

/////////////////////////////////////////
//
//GPS Setting
//
/////////////////////////////////////////
XBeeAddress64 collect_Address[5];
XBeeAddress64 robot_Address

void GPS_Setting(Timer refresh_Timer, Timer collect_Timer){
    gps_Serial = new Serial(p28,p27);
    Adafruit_GPS myGPS(gps_Serial); 
    Timer refresh_Timer;
    const int refresh_Time = refresh_Timer; //refresh time in ms
    myGPS.begin(GPS_BAUD_RATE); 
    
    Timer collect_Timer;
    const int collect_Time = collect_Timer; //when we collect 4 GPS point, we use 
    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)};
    robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
    }

/////////////////////////////////////////
//
//GPS Send Command
//
/////////////////////////////////////////
void GPS_SendCommand(){
    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
    myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
    myGPS.sendCommand(PGCMD_ANTENNA);
    }
    
/////////////////////////////////////////
//
//Check Command Type
//
/////////////////////////////////////////
void check_CommandType(utf8_t command_type, AigamozuControlPackets ai(motorShield), ){
    switch(command_type){
                    
                    //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:{
                           
                            //send normal data
                            //Create GPS Infomation Packet
                            agz.createReceiveStatusCommand(Base_ID,'a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
                            //Select Destination
                            ZBTxRequest tx64request2(collect_Address[id],ai.packetData,ai.getPacketLength());
                            //Send -> Base
                            xbee.send(tx64request);
                            
                            //send Kalman data
                            agz.createReceiveStatusCommandwithKalman(Base_ID,'a',myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
                            //Select Destination
                            ZBTxRequest tx64request3(collect_Address[id],agz.packetData,agz.getPacketLength());
                            //Send -> Base
                            xbee.send(tx64request);
                            break;         
          
                        }

                    default:{
                        break;
                    }
                }
            }

/////////////////////////////////////////
//
//latitude and longitude after filtering
//
/////////////////////////////////////////
void LatLong_afterFiltering(double longitude, double latitude, double x0, double x1){
    myGPS.longitudeKH=longitude;//longitude after filtering
    myGPS.latitudeKH=latitude;//latitude after filtering
    myGPS.longitudeKL=(long)x0;//longitude after filtering
    myGPS.latitudeKL=(long)x1;//latitude after filtering
}

/////////////////////////////////////////
//
//send_GPSdata()
//
/////////////////////////////////////////
void send_GPSdata(){
    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';+
                           
        //Check Command Type 
        check_CommandType(agz.checkCommnadType(buf));
            
    }
}

/////////////////////////////////////////
//
//get_GPSdata
//
/////////////////////////////////////////
void get_GPSdata(){
    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;
}

/////////////////////////////////////////
//
//get_Kalman
//
/////////////////////////////////////////
void get_Kalman(){
    //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];
        }
    }
                
    LatLong_afterFiltering(myGPS.longitudeH, myGPS.latitudeH, x[1][0], x[1][1]);
    }
/////////////////////////////////////////
//
//Main Processing
//
/////////////////////////////////////////
int main() {
    setting();
    
    //GPS setting
    GPS_Setting(2000, 200);
    int collect_flag = 0;
    char collect_state = 'a';
    int id;
    bool flag = true;
    
    //GPS Send Command
    GPS_SendCommand();
    
    wait(2);
       
    //interrupt start
    interrupt_start(agz_motorShield, refreshTime);
    
    while (true) {
        
        //Check Xbee Buffer Available
        xbee.readPacket();
            if (xbee.getResponse().isAvailable()) {
                send_GPSdata();
            }
           
        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;
                
                get_GPSdata();
                
                get_Kalman();
                
                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;
            
        } 
        
    
    } 
}