File content as of revision 9:357fa70be9c7:

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

#include "mbed.h"
#include "XBee.h"
#include "MBed_Adafruit_GPS.h"
#include "AigamozuControlPackets.h"
#include "agzIDLIST.h"
#include "aigamozuSetting.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(PAN1B1_32H,PAN1B1_32L);

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

/////////////////////////////////////////
//
//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); 
    
    //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();
    
    
    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(); 
                 
                 
                 //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:{
                            //Create GPS Infomation Packet
                            agz.createReceiveStatusCommand('B','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
                            //Select Destination
                            ZBTxRequest tx64request(remoteAddress,agz.packetData,agz.getPacketLength());
                            //Send -> Base
                            xbee.send(tx64request);
                            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;
                agz.reNewPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
            }
            else agz.nowStatus = GPS_UNAVAIL;
        } 
        

        
            
    }
}