File content as of revision 3:693ea9476763:

/**********************************************/
//
//
//
//  Program name: Aigamozu ROBOT
//  Author: Mineta Kizuku
//
//
//
/**********************************************/

/**********************************************/
//更新情報
//2015/05/11
//ロボットプログラムの作成
//
//2015/05/13
//カルマンフィルタの共分散の値を0.0001以下にならないようにした
//共分散の値を10進数に変換するようにした
//
//2015/05/13 Yokokawa
//何回目のGPSでとられたGPSか確認するようにしました。
//
//2015/05/15
//プログラムcreateReceiveStatusCommand()にて
//Aigamozu_collect_dataにinかoutかを送るためにstate関連をいじったので必要に応じて直してください。
//
//2015/05//17
//Send_Status関数内を変更：基地局への送信データのみ現在のモードを入れるパケットの部分に内外判定の結果を入れるようにした
//基地局以外には現在のモードを送信するようにしてある
//要確認！！！！
//
//2015/05/17
//Get_GPS()の中身longitudeの範囲138〜140に変更
//
//2015/05/19
//autoモードのとき、三十秒前進・三秒右に転回に変更した。
//
//2015/05/24
//Kalmanフィルターを十進数で計算するようにした。
//Kalmanフィルターの計算式を変更した。
//set_kalmanを追加した。
//
/**********************************************/

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

#define SIGMA_MIN 0.0001
#define KALMAN_START_TIME 5

//************ID Number*****************
//Robot   ID: 'A' ~ 'Z'
//Base    ID: 'a' ~ 'z'
//manager ID: '0'(数字のゼロ)
//
const char MyID = 'B';
const char Collect_Number = 'a';
//************ID Number*****************

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


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

//set up GPS module

//set up AigamozuControlPackets library
AigamozuControlPackets agz(agz_motorShield);


/////////////////////////////////////////
//
//For Kalman data
//
/////////////////////////////////////////
#define FIRST_S1 0.0001
#define FIRST_S2 0.000001
#define COUNTER_MAX 10000
double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
double s2x_cur=FIRST_S1,s2x_prev=FIRST_S1,s2y_cur=FIRST_S1,s2y_prev=FIRST_S1;//緯度経度のの時刻tと時刻t-1での共分散
double s2_R=FIRST_S2;//GPSセンサの分散
double Kx=0,Ky=0;//カルマンゲイン
double zx,zy;//観測値
void Kalman(double Latitude,double Longitude);
int change = 0;

/*
LocalFileSystem local("local");  // マウントポイントを定義（ディレクトリパスになる）
FILE *fp;
char filename[16] = "/local/out0.txt";
*/
/////////////////////////////////////////
//
//Plus Speed
//
//MNUAL_MODEの時にスピードを変える
/////////////////////////////////////////
void Plus_Speed(uint8_t *packetdata)
{

    if(agz.nowMode == MANUAL_MODE) {
        agz.changeSpeed(packetdata);
    }

}


/////////////////////////////////////////
//
//Send Status
//
//リクエストがきたとき、自分の位置情報などを返信する
/////////////////////////////////////////
void Send_Status(char SenderIDc)
{
    XBeeAddress64 send_Address;
    if(SenderIDc == '0') {
        send_Address = manager_Address;
        agz.createReceiveStatusCommand(MyID,SenderIDc, agz.nowMode,
                                       agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
                                       agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
                                       agz.get_agzCov_lati(),agz.get_agzCov_longi());
    }
    if(SenderIDc >= 'A' && SenderIDc <= 'Z') {
        send_Address = robot_Address[SenderIDc - 'A'];
        //Create GPS Infomation Packet
        agz.createReceiveStatusCommand(MyID,SenderIDc, agz.nowMode,
                                       agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
                                       agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
                                       agz.get_agzCov_lati(),agz.get_agzCov_longi());
    }
    if(SenderIDc >= 'a' && SenderIDc <= 'z') {
        send_Address = base_Address[SenderIDc - 'a'];

        agz.createReceiveStatusCommand(MyID,SenderIDc, (int)agz.gpsAuto(),
                                       agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
                                       agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
                                       agz.get_agzCov_lati(),agz.get_agzCov_longi());
    }
    //send normal data

    /*
       //debug***************************************************
       printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
               agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
               agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
               agz.get_agzCov_lati(),agz.get_agzCov_longi()
               );
       for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
           printf("\n");
       //debug end***************************************************
    */
    //Select Destination
    ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
    //Send -> Base
    xbee.send(tx64request);

}

/////////////////////////////////////////
//
//Get GPS function
//
/////////////////////////////////////////

void Get_GPS(Adafruit_GPS *myGPS)
{
    static int flag = 0;
//    static int save_counter = 0;

    if (myGPS->fix) {

        agz.nowStatus = GPS_AVAIL;

            /*            if(save_counter < 10){
                            fp = fopen(filename, "a");
                            fprintf(fp, "%d %.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
                                change, agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
                                agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
                                agz.get_agzCov_lati(),agz.get_agzCov_longi());
                            fclose(fp);

                            if((flag - 16) % 500 == 0){
                                filename[10]++;
                                save_counter++;
                            }
                        }
            */

    } else agz.nowStatus = GPS_UNAVAIL;

}

/////////////////////////////////////////
//
//New Mode
//
/////////////////////////////////////////

void New_Mode(uint8_t *packetdata)
{

    //bool result;
    agz.changeMode(packetdata);

}

/////////////////////////////////////////
//
//Get Status
//
/////////////////////////////////////////
void Get_Status(char SenderIDc,uint8_t *packetdata)
{
    int id = 0;

        //printf("Get data\n");
        agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]);
        agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
        agz.set_agzCov_from_packet(&packetdata[45],&packetdata[53]);


        printf("%d, %f, %f\n",
               SenderIDc, agz.get_basePoint_lati(id),agz.get_basePoint_longi(id)
              );
    

}


/////////////////////////////////////////
//
//Send_Request
//
/////////////////////////////////////////
void Send_Request(int number)
{

    agz.createRequestCommand(MyID, number);
    //Select Destination!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    //基地局の場合
    if('a' <= number && number <= 'z'){
    number = number - 'a';
    //printf("Send b:%d\n",number);
    ZBTxRequest tx64request(base_Address[number],agz.packetData,agz.getPacketLength());
    //Send
    xbee.send(tx64request);
    }
    //ロボットの場合
    if('A' <= number && number <= 'Z'){
    number = number - 'A';
    //printf("Send r:%d\n",MyID);
    ZBTxRequest tx64request(robot_Address[number],agz.packetData,agz.getPacketLength());
    //Send
    xbee.send(tx64request);
    }

}


void print_gps(int count)
{

    printf("%d times:\n", count);
    printf("%f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());

}


/////////////////////////////////////////
//
//Kalman Processing
//
/////////////////////////////////////////
void calc_Kalman()
{
    //calc Kalman gain
    Kx = s2x_prev/(s2x_prev+s2_R);
    Ky = s2y_prev/(s2y_prev+s2_R);
    //estimate
    x_cur = x_prev + Kx*(zx-x_prev);
    y_cur = y_prev + Ky*(zy-y_prev);
    //calc sigma
    s2x_cur = s2x_prev-Kx*s2x_prev;
    s2y_cur = s2y_prev-Ky*s2y_prev;

}

void Kalman(double Latitude,double Longitude)
{

    zx = Latitude;
    zy = Longitude;

    calc_Kalman();

    //更新
    x_prev = x_cur;
    y_prev = y_cur;
    s2x_prev = s2x_cur;
    s2y_prev = s2y_cur;

    //agzPontKalmanとagzCovに格納する
    agz.set_agzPointKalman_lati(x_cur);
    agz.set_agzPointKalman_longi(y_cur);
    agz.set_agzCov(s2x_cur,s2y_cur);

}

/////////////////////////////////////////
//
//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;
    Timer auto_Timer;
//    const int straight = 8000, turning = 12000, wait = 10000;

    myGPS.begin(GPS_BAUD_RATE);

    Timer collect_Timer;
    const int collect_Time = 1000; 

    char SenderIDc;
     wait_ms(2000);
    //GPS Send Command
    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_ALLDATA);
    myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
    myGPS.sendCommand(PGCMD_ANTENNA);
        wait_ms(2000);
        
    //interrupt start
    refresh_Timer.start();
    auto_Timer.start();
    agz.Move_Timer.start();
    collect_Timer.start();
    agz.Automove_Timer.start();
    printf("start\n");

    agz.auto_count = 0;
    
 //   myGPS.count_[0] = 3;
 //   myGPS.count_[1] = 3;
 //   myGPS.count_[2] = 3;
 myGPS.print_ok = 0;

    while (true) {

        //Check Xbee Buffer Available
        xbee.readPacket();

        if (xbee.getResponse().isAvailable()) {
            xbee.getResponse().getZBRxResponse(zbRx);
            uint8_t *buf = zbRx.getFrameData();

            if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
                xbee.getResponse().getZBRxResponse(zbRx);
                uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
                uint8_t *buf1 = &buf[11];//データの部分のみを格納する
                SenderIDc = buf1[5];//送信元のIDを取得する
                char Command_type = agz.checkCommnadType(buf);//コマンドタイプを取得する

                //Check Command Type
                switch(Command_type) {
                        //Get Request command
                    case MANUAL: {
                        Plus_Speed(buf);
                        break;
                    }
                    case STATUS_REQUEST: {
                        Send_Status(SenderIDc);
                      //  printf("%c\n", SenderIDc);
                        break;
                    }
                    case CHANGE_MODE: {
                        New_Mode(buf);
                        break;
                    }
                    case RECEIVE_STATUS: {
                        //printf("GET");
                        Get_Status(SenderIDc,buf1);
                        break;
                    }
                    default: {
                        break;
                    }
                }//endswitch
            }//endifZB_RX_RESPONSE
        }//endifisAvailable


             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;   
            }else{
 
            }    
            if(strstr(myGPS.lastNMEA(), "$GPGSA")){
                //myGPS.GPGSAdata=myGPS.lastNMEA();
                //printf("%s",myGPS.GPGSAdata);
            }
            if(strstr(myGPS.lastNMEA(), "$GPGGA")){
                //myGPS.GPGGAdata=myGPS.lastNMEA();
                //printf("%s",myGPS.GPGGAdata);
            }
            if(strstr(myGPS.lastNMEA(), "$GPRMC")){
                //myGPS.GPRMCdata=myGPS.lastNMEA();
                //printf("%s",myGPS.GPRMCdata);
            }
                                
            //printf("%s",myGPS.GPGGAdata);               
            //printf("%s",myGPS.GPRMCdata);
            //printf("%s",myGPS.GPGSAdata);
        }
        

        //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
//   if (refresh_Timer.read_ms() >= 1000){
        //    refresh_Timer.reset();
            //print_gps(count);

     //   }

        
    }

}