File content as of revision 38:5cd6d4964f65:

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



int change = 0;

/////////////////////////////////////////
//
//Plus Speed
//
//MNUAL_MODEの時にスピードを変える
/////////////////////////////////////////
void Plus_Speed(uint8_t *packetdata){
       
    if(agz.nowMode == MANUAL_MODE){ 
        agz.changeSpeed(packetdata);
    }
    
}



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

void New_Mode(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); 
    
    Timer auto_Timer;
    const int auto_Time = 2000; //refresh time in ms
    
    wait_ms(2000);
       
    //interrupt start
    auto_Timer.start();
    agz.Move_Timer.start();
    printf("start\n");
    
    
    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();//フレームデータを格納する
                char Command_type = agz.checkCommnadType(buf);//コマンドタイプを取得する
                 
                //Check Command Type 
                switch(Command_type){
                    //Get Request command
                    case MANUAL:{
                        Plus_Speed(buf);
                        break;
                    }
                    case STATUS_REQUEST:{                 
                        break;          
                    }
                    case CHANGE_MODE:{
                        New_Mode(buf);
                        break;
                    }
                    case RECEIVE_STATUS:{
                        break;
                    }
                    default:{
                        break;
                    }
                }//endswitch
            }//endifZB_RX_RESPONSE
        }//endifisAvailable
  
     if(agz.nowMode == AUTO_GPS_MODE && auto_Timer.read_ms() >= auto_Time){
           auto_Timer.reset();
           agz.gpsAuto();
        }
        
    }
    
}