File content as of revision 3:7affc8af6db6:

/**********************************************/
//  
//    
//
//  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"
#include "agz_common.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[4];

/////////////////////////////////////////
//
//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); 
    
    Timer collect_Timer;
    const int collect_Time = 200; //when we collect 4 GPS point, we use 
    int collect_flag = 0;
    char collect_state = 'a';
    XBeeAddress64 collect_Address[4] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 
                                        XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L)};
    XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
    int id;
  
    
    //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();
    
    printf("test\n");
        
    
    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();
                uint8_t *buf1 = &buf[11]; 
                 
                 
                 //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('D','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
                            //Select Destination
                                ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength());
                            //Send -> Base
                            xbee.send(tx64request);
                            break;         
          
                        }
                        
                    case RECEIVE_STATUS:{
                            
                        //printf("Receive Status\n");      
                           
                        id = buf1[5] - 'A';
                        robot[id].set_state(buf1[9]);
                        robot[id].set_LatitudeH(&buf1[13]);
                        robot[id].set_LatitudeL(&buf1[17]);
                        robot[id].set_LongitudeH(&buf1[21]);
                        robot[id].set_LongitudeL(&buf1[25]); 
                        
                        agz.reNewBasePoint(id,robot[id].get_LatitudeH(),robot[id].get_LatitudeL(),robot[id].get_LongitudeH(),robot[id].get_LongitudeL());
                    /*   
                        printf("%d,", buf1[5]);                    
                        printf(" %ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL());
                    */
                        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.reNewRobotPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
                //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;
            
        } 
        
        //get base GPS
        if( collect_Timer.read_ms() >= collect_Time){
            collect_Timer.reset();
            
            //printf("Send Request:%d,%d\n",collect_flag,collect_state);
                      
            agz.createRequestCommand('A', collect_state);
            //Select Destination
            ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength());
            //Send -> Base
            xbee.send(tx64request);
            
            collect_flag++;
            collect_state++;

            if(collect_flag == 4){
                collect_state = 'a';
                collect_flag = 0;
            }    
         } 
    } 
}