File content as of revision 2:95955f38f47a:

/**********************************************/
//
//
//  Program name: Aigamozu Robot Control
//  author: Atsunori Maruyama
//  
//
//  ver ->  1.0
//  
//
/**********************************************/

#include "mbed.h"
#include "XBee.h"
#include "MBed_Adafruit_GPS.h"
#include "VNH5019.h"
#include "agzIDLIST.h"


#define GPS_BAUD_RATE 9600
#define XBEE_BAUD_RATE 57600
#define PC_BAUD_RATE 57600
#define IMU_BAUD_RATE 400000

/////////////////////////////////////////
//
//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
//
/////////////////////////////////////////

//Motor Contorol Pin Setting
VNH5019 motorShield(p21,p22,p23,p24,p25,p26);

//Interrupt Setting
Ticker axis;
Ticker auth_axis;

/////////////////////////////////////////
//
//Each Value Setting
//
/////////////////////////////////////////
int count = 0;

//my status 
//0: StndbyMode
//1: ManualMode
//2: AuthmaticMode(Random)
unsigned char my_status = 0;

//0 bit: Motor Satatus
//1 bit: GPS Status
//2 bit: Sensor Status 
//3 bit: Battery Status
unsigned char my_mode = 0;
     
//ManualValue
int manual_count = 0;
int manual_flag = 0;

//test value
long sub_latH = 12345;
long sub_latL = 67890;
long sub_lonH = 98765;
long sub_lonL = 43211;


union UNI_TEST_T
{
    long a;
    uint8_t b[4];
};

UNI_TEST_T latH,latL,lonH,lonL;


/////////////////////////////////////////
//
//Interruption processing 1: time -> 0.1s
//
/////////////////////////////////////////
void axisRenovation(){
        
    if(manual_count > 100){
        motorShield.changeSpeed(0,0,0,0);
        manual_count = 0;
    }
    if(my_mode == 1) manual_count++;
    }
    
/////////////////////////////////////////
//
//Interruption processing: time -> 1.0s
//
/////////////////////////////////////////
 
 void randomRenovation(){
    
    if(count < 20){
        motorShield.changeSpeed(1,127,1,127);
    }
    
    else{
        motorShield.changeSpeed(1,64,2,64);
        if(count > 21) {
            
            count = 0;
            motorShield.changeSpeed(1,127,1,127);
            
            }
        } 
    count++;
    }   



/////////////////////////////////////////
//
//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);
    
    //gyro_registor Setting
    wait(2);
       
    //interrupt start
    axis.attach(&axisRenovation, 0.1);
    refresh_Timer.start();
    
    /// @todo change to true
    while (true) {
        
        //recive xbee module 
        xbee.readPacket();
            if (xbee.getResponse().isAvailable()) {
                if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
                xbee.getResponse().getZBRxResponse(zbRx);
                unsigned char *buf = zbRx.getFrameData(); 
                 
                 /// @todo use preporcessor (#define) or const.
                 /// @note It's magic number.
                 // COMMAND_TYPE commandType = buf[14];
                 /// switch(commandType)
                 /// {case MANUAL:}
                 
                 
                 //switch(aigamoCTR.checkCommandType(buf)){
                 switch((unsigned char)buf[14]){
                    //ChanegeMode
                    case 'C':{
                        my_mode = buf[19];
                        motorShield.changeSpeed(0,0,0,0);
                        if(my_mode == 2)  auth_axis.attach(&randomRenovation, 1.0);
                        else auth_axis.detach();
                        break;
                        }
                    
                    /*
                    case 'C':
                    agz.change_Mode();
                    break;
                    */
                    
                    //MunualCommand
                    case 'M':{  
                            /// @todo Magic.
                            if(my_mode == 1){ 
                            manual_count = 0;
                            motorShield.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
                            }
                        break;
                        }
                    
                    /*
                    case 'M':
                    uint8_t sppeds_buf[4] = agz.parseMunualCommand(buf);
                    motorShield.changeSpeed(sppedbuf[0],sppedbuf[1],sppedbuf[2],sppedbuf[3]);
                    break;
                    */
                    
                    //SendStatus
                    case 'S':{
                            latH.a = myGPS.latitudeH;;
                            latL.a = myGPS.latitudeL;
                            lonH.a = myGPS.longitudeH;
                            lonL.a = myGPS.longitudeL;
                            //dummy data
                            //latH.a = sub_latH;latL.a = sub_latL;lonH.a = sub_lonH;lonL.a = sub_lonL;
                            
                            // if(validateCommand(commandType));
                            // createReceiveStatusData();
                            uint8_t data[] = {65,71,83,82,70,'2',84,'A',83,my_status,71,80,83,latH.b[0],latH.b[1],latH.b[2],latH.b[3],latL.b[0],latL.b[1],latL.b[2],latL.b[3],lonH.b[0],lonH.b[1],lonH.b[2],lonH.b[3],lonL.b[0],lonL.b[1],lonL.b[2],lonL.b[3],65,71,69};
                            ZBTxRequest tx64request(remoteAddress,data,sizeof(data));
                            xbee.send(tx64request);
                            break;         
                    /*
                    case 'S':
                    */        
                        }
                    
                        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) my_status = 0;
            else my_status = 1;
        }     
    }
}