File content as of revision 3:1229ca3df855:

/**********************************************/
//  
//    
//
//  Program name: Aigamozu Robot Control
//  author: Atsunori Maruyama
//  ver ->  1.0
//  day -> 2014/05/21
//
//  
//
/**********************************************/
#include "mbed.h"
#include "XBee.h"
#include "MBed_Adafruit_GPS.h"
#include "VNH5019.h"
#include "agzIDLIST.h"
#include "aigamozuSetting.h"
#include  "AigamozuControlPackets.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
//
/////////////////////////////////////////
//Motor Contorol Pin Setting
VNH5019 motorShield(p21,p22,p23,p24,p25,p26);
AigamozuControlPackets agz;


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

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

    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(); 
                 
                 
                 //Analyze the command type
                 switch(agz.checkCommnadType(buf)){
                    
                    //CommnadType -> ChangeModeCommand;;;
                    case CHANGE_MODE:{
                        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;
                        }
                    
                    //CommnadType -> ManualCommand;;;
                    case MANUAL:{  
                            /// @todo Magic.
                            if(my_mode == 1){ 
                            manual_count = 0;
                            motorShield.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
                            }
                        break;
                        }
                    
                    //SendStatus
                    case STATUS_REQUEST:{
                            //Create packet
                            uint8_t* data = agz.createStatusPacket(myGPS.latitudeH, myGPS.latitudeL, myGPS.longitudeH, myGPS.longitudeL,1,2);
                            ZBTxRequest tx64request(remoteAddress,data,sizeof(data));
                            //Send GPS ingomation
                            xbee.send(tx64request);
                            break;          
                        }
                        
                    //Default
                        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;
        }     
    }
}