File content as of revision 5:e1b1b9a43ccb:

#include "mbed.h"
#include "XBee.h"
#define BUFFER_SIZE 512

//pin set
//Serial,I2C
I2C i2c(p9, p10);        //sda, scl,MFU
Serial pc(USBTX, USBRX); //tx, rx
Serial gps(p28, p27);    //tx, rx
XBee xbee(p13,p14);    //tx,rx
ZBRxResponse zbRx = ZBRxResponse();


//motor
DigitalOut motorL_A(p21);
DigitalOut motorL_B(p22);
PwmOut motorL_pwm(p23);

DigitalOut motorR_A(p24);
DigitalOut motorR_B(p25);
PwmOut motorR_pwm(p26);

//encorder
InterruptIn encoder1_A(p15);
InterruptIn encoder1_B(p16);

Ticker axis;

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

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

//motor_speed_feed_back
float target_palse = 0.0;
float pwm;
long encorder_count = 0;

//GPS
//Ring Buffer
typedef unsigned char  UBYTE;
typedef unsigned int UWORD;

typedef struct {
  UWORD cap;
  UWORD wp;
  UWORD last_head_rp;
  UWORD head_rp;
  UWORD rp;
  UBYTE dat[BUFFER_SIZE];
} RINGP;

RINGP gps_buf;

UBYTE get_ring(void){
    UBYTE result ;
    /* load data */
    result = gps_buf.dat[gps_buf.rp] ;
    /* update pointer */
    gps_buf.rp += 1 ;
    if ( gps_buf.rp == BUFFER_SIZE ) { gps_buf.rp = 0 ; }
    /* capacity decrement */
    gps_buf.cap -= 1 ;

    return result ;
}

void put_ring(UBYTE x){
    /* store data */
    gps_buf.dat[gps_buf.wp] = x ;
    /* update pointer */
    gps_buf.wp += 1 ;
    if ( gps_buf.wp == BUFFER_SIZE ) { gps_buf.wp = 0 ; }
    /* capacity increment */
    gps_buf.cap += 1 ;
}

void init_ring(void){
    gps_buf.cap=0;
    gps_buf.wp=0;
    gps_buf.last_head_rp=0;
    gps_buf.head_rp=0;
    gps_buf.rp=0;
}

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];
    
    //xbee.printf("%d, %d, %d ,", gyro_x, gyro_y, gyro_z);
    //xbee.printf("%d, %d, %d", acc_x, acc_y, acc_z);
    //xbee.printf("%ld,%lf,%lf\n",encorder_count,target_palse,pwm);
    
    
    //feedback
    //pwm = 0.01 * (target_palse - encorder_count/11264.0) + motor1_pwm;
    //motor1_pwm  = pwm;
    //encorder_count = 0;
    
    //Output GPS data -> xbee
    /*
    char val;
    gps_buf.rp = gps_buf.last_head_rp; 
    while(1){
        val = get_ring();
        xbee.printf("%c",val);
        if(val == 0x0a) break;   
        }
      */  
    }
    
void gps_rx(){
    char val = gps.getc();
    put_ring(val); 
    if(val == '$'){
        gps_buf.last_head_rp = gps_buf.head_rp;
        gps_buf.head_rp = gps_buf.wp;
        }    
    }
    
    /*
void xbee_rx(){
    char val = xbee.getc();
    if(val == 'a') {
        motor1_A = 1;
        motor1_B = 0;
        motor1_pwm = 0.5;
        //motor2_A = 1;
        //motor2_B = 0;
        }
    else if(val == 's'){
        motor1_A = 0;
        motor1_B = 0;
        motor1_pwm = 0.0;
        //motor2_A = 0;
        //motor2_B = 0;       
        }
    else if(val == 'd'){    
    target_palse = target_palse + 0.1;
    }
     else if(val == 'f'){    
    target_palse = target_palse - 0.1;
    }   
        
    
    }
*/

void change_speed(unsigned char L_state,unsigned char L_pwm,unsigned char R_state,unsigned char R_pwm){
    
    switch(L_state){
        case 0:
            motorL_A = 0;
            motorL_B = 0;
        
        case 1:
            motorL_A = 1;
            motorL_B = 0;
        
        case 2:
            motorL_A = 0;
            motorL_B = 1;        
        }
    
    motorL_pwm = (float)L_pwm/256.0;
    
    
    
    switch(L_state){
        case 0:
            motorR_A = 0;
            motorR_B = 0;
        
        case 1:
            motorR_A = 1;
            motorR_B = 0;
        
        case 2:
            motorR_A = 0;
            motorR_B = 1;        
        }
    
    motorR_pwm = (float)R_pwm/256.0;
    
    
    
    
    
    
    
    }

void send_GPS_command(){
    char gps_command1[] = "$PMTK220,500*2B";
    char gps_command2[] = "$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29";
    
    char CR = 0x0d;
    char LF = 0x0a;

    gps.printf("%s%c%c",gps_command1,CR,LF);
    gps.printf("%s%c%c",gps_command2,CR,LF);
    
    }


void flip(){
    encorder_count++;
    }


int main() {
    //start up time
    wait(1);
    //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
    send_GPS_command();
    init_ring();
    gps.attach(gps_rx,Serial::RxIrq);
    //xbee.attach(xbee_rx,Serial::RxIrq);
    wait(5);

    //Encorder Innterrapt Setting
    //encoder1_A.rise(&flip);
    
    //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(1);
    
    //interrupt start
    axis.attach(&axisRenovation, 0.1);
    motorL_pwm = 0.0;
    motorR_pwm = 0.0;
    
    
    while (1) {
        
        
        xbee.readPacket();
            if (xbee.getResponse().isAvailable()) {
                if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
                xbee.getResponse().getZBRxResponse(zbRx);
                unsigned char *buf = zbRx.getFrameData(); 
                
                /*for(int i = 0 ;i < zbRx.getPacketLength(); i++){
                    pc.printf("%d\n",buf[i]);
                    }
                    */
                      //pc.printf("%c%c%c\n",buf[11],buf[12],buf[13]);
                      change_speed(buf[17],buf[18],buf[20],buf[21]);
                      
                      
            }
    
            
            
        }
     
        
    }
    
}