File content as of revision 49:a6ae71759d0f:

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

/**********************************************/
//更新情報
//展示会用プログラム
//中身をすっきりさせました。
//Automodeの際のプログラムはgpsAuto()の中身を変更すれば大丈夫です。
/**********************************************/

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

/////////////////////////////////////////
//
//Pin Setting
//
/////////////////////////////////////////
//Serial Connect Setting: mbed <--> motor driver
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 AigamozuControlPackets library
AigamozuControlPackets agz(agz_motorShield);


/////////////////////////////////////////
//
//Plus Speed
//
//ロボットを操縦する
/////////////////////////////////////////
void plusSpeed(uint8_t *packetdata){
       
    if(agz.nowMode == MANUAL_MODE){ 
        agz.changeSpeed(packetdata);
    }
    
}



/////////////////////////////////////////
//
//New Mode
//
//ロボットのモードを変更する
/////////////////////////////////////////
void newMode(uint8_t *packetdata){
  
  //bool result;
   agz.changeMode(packetdata); 
  
}


/////////////////////////////////////////
//
//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); 
  
    //2secごとの更新用Timer
    Timer autoTimer;
    const int autoTime = 1000; //refresh time in ms

    //interrupt start
    autoTimer.start();
    agz.moveTimer.start();
    
    wait_ms(2000);
    
    printf("start\n");    

    while (true) {
        
        //Check Xbee Buffer Available
        xbee.readPacket();
        
        //Xbee受信があった場合
        if (xbee.getResponse().isAvailable()) {
            xbee.getResponse().getZBRxResponse(zbRx);
            //frame dataの格納
            uint8_t *buf = zbRx.getFrameData();
            
            if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
                xbee.getResponse().getZBRxResponse(zbRx);
                uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
                char Command_type = agz.checkCommnadType(buf);//コマンドタイプを取得する
                 
                //Check Command Type 
                switch(Command_type){
                    //Get Request command
                    case MANUAL:{ //マニュアルモードパケット
                        plusSpeed(buf); //ロボットの操縦
                        break;
                    }
                    case STATUS_REQUEST:{ //ステータスリクエストパケット           
                        break;          
                    }
                    case CHANGE_MODE:{ //モード変更パケット
                        newMode(buf); //モードの変更
                        break;
                    }
                    case RECEIVE_STATUS:{ //レシーブステータスパケット
                        break;
                    }
                    default:{
                        break;
                    }
                }//endswitch
            }//endifZB_RX_RESPONSE
        }//endifisAvailable
  
  //ロボットがオートモードのとき、シーケンス動作する
     if(agz.nowMode == AUTO_GPS_MODE && autoTimer.read_ms() >= autoTime){
           autoTimer.reset();
           agz.autoMove();
        }
        
    }
    
}