File content as of revision 1:490b793b2e61:

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

/////////////////////////////////////////
//
//Connection Setting
//
/////////////////////////////////////////
DigitalOut led1(LED1);
//IMU -> i2c
I2C i2c(p9, p10);
//set serial
Serial pc(USBTX, USBRX); //tx, rx
XBee xbee(p13,p14);    //tx,rx
Serial * gps_Serial;
//gps responce
ZBRxResponse zbRx = ZBRxResponse();
XBeeAddress64 remoteAddress = XBeeAddress64(0x0013A200,0x40991C67);


/////////////////////////////////////////
//
//Pin Setting
//
/////////////////////////////////////////

//Motor & Encorder Setting
VNH5019 motorShield(p21,p22,p23,p24,p25,p26);

//test data
Ticker axis;
Ticker auth_axis;

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

//my_status
// 0 -> stable
// 1 -> error

//0 bit: Motor Satatus
//1 bit: GPS Status
//2 bit: Sensor Status 
//3 bit: Battery Status
//4 bit: 
//5 bit:
//6 bit:
//7 bit:

unsigned char my_mode = 0;

//I2C address 9-axis
const int gyro_addr = 0xD0;
const int acc_addr = 0xA6;

char gyro_head[1];
char read[8];
short int gyro_x=0;
short int gyro_y=0;
short int gyro_z=0;
short int tempr=0;

char acc_head[1];
char acc_buf[6];
short int acc_x = 0;
short int acc_y = 0;
short int acc_z = 0;
    
//GPS value
//float longitude = 0.0;
//float latitude = 0.0;    
     
//motor_speed_feed_back
float target_palse = 0.0;
float pwm;
long encorder_count = 0;
int count = 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(){
    //gyro
    gyro_head[0] = 0x1B;
    i2c.write(gyro_addr, gyro_head, 1);
    i2c.read(gyro_addr, read, 8);

    tempr=(read[0] << 8) + read[1];
    gyro_x=(read[2] << 8) + read[3];
    gyro_y=(read[4] << 8) + read[5];
    gyro_z=(read[6] << 8) + read[7];
        
    //acc
    acc_head[0] = 0x32;
    i2c.write(acc_addr,acc_head,1);
    i2c.read(acc_addr, acc_buf, 6);
        
    acc_x = (acc_buf[1] << 8) + acc_buf[0];
    acc_y = (acc_buf[3] << 8) + acc_buf[2];
    acc_z = (acc_buf[5] << 8) + acc_buf[4];
    
    
    if(manual_count > 100){
        motorShield.changeSpeed(0,0,0,0);
        manual_count = 0;
    }
    
    manual_count++;
    
    //printf("Mode -> %d\n", my_mode);
    //printf("Status -> %d\n", my_status);
    //printf("Status -> %d\n", count);

    }
    
/////////////////////////////////////////
//
//Interruption processing: time -> 1.0s
//
/////////////////////////////////////////
 
 void randomRenovation(){
    
    if(count < 30){
        motorShield.changeSpeed(1,127,1,127);
    }
    
    else{
        motorShield.changeSpeed(1,127,2,127);
        if(count > 35) count = 0;
        } 
    count++;
    }   



/////////////////////////////////////////
//
//Main Processing
//
/////////////////////////////////////////
int main() {
    //start up time
    wait(3);
    //set i2c frequency to 400 KHz
    i2c.frequency(400000); 
    //set pc frequency to 57600bps 
    pc.baud(57600); 
    //set xbee frequency to 57600bps
    xbee.begin(57600);    
    
    //GPS setting
    gps_Serial = new Serial(p28,p27); //serial object for use w/ GPS
    Adafruit_GPS myGPS(gps_Serial); //object of Adafruit's GPS class
    //char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
    Timer refresh_Timer; //sets up a timer for use in loop; how often do we print GPS info?
    const int refresh_Time = 2000; //refresh time in ms
    
    myGPS.begin(9600);  //sets baud rate for GPS communication; note this may be changed via Adafruit_GPS::sendCommand(char *)
                        //a list of GPS commands is available at http://www.adafruit.com/datasheets/PMTK_A08.pdf
    
    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); //these commands are defined in MBed_Adafruit_GPS.h; a link is provided there for command creation
    myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
    myGPS.sendCommand(PGCMD_ANTENNA);
    
    //gyro_registor Setting
    char PWR_M[2]={0x3E,0x80};
    i2c.write(gyro_addr, PWR_M, 2); // Send command string
    char SMPL[2]={0x15,0x00};
    i2c.write(gyro_addr, SMPL, 2); // Send command string
    char DLPF[2]={0x16,0x18};
    i2c.write(gyro_addr, DLPF, 2); // Send command string
    char INT_C[2]={0x17,0x05};
    i2c.write(gyro_addr, INT_C, 2); // Send commanad string
    char PWR_M2[2]={0x3E,0x00};
    i2c.write(gyro_addr, PWR_M2, 2); // Send command string
    
    wait(2);
       
    //interrupt start
    axis.attach(&axisRenovation, 0.1);
    
    refresh_Timer.start();
    
    while (1) {
        
        //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(); 
                 
                 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;
                        }
                    
                    //MunualCommand
                    case 'M':{  
                            if(my_mode == 1){ 
                            manual_count = 0;
                            motorShield.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
                            
                            }
                        break;
                        }
                    
                    //SendStatus
                    case 'S':{
                            //latH.a = myGPS.latitudeH;;
                            //latL.a = myGPS.latitudeL;
                            //lonH.a = myGPS.longitudeH;
                            //lonL.a = myGPS.longitudeL;
                            led1 = 1;
                            //dummy data
                            latH.a = sub_latH;
                            latL.a = sub_latL;
                            lonH.a = sub_lonH;
                            lonL.a = sub_lonL;
                        
                            uint8_t data[] = {65,71,83,82,70,65,84,66,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;            
                        }
                    
                        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;
                //longitude = myGPS.longitudeH;
                //latitude = myGPS.latitudeH;       
            }
            
            else my_status = 1;
        }     
    }
}