File content as of revision 2:f8bfd160f174:

#include "mbed.h"
#define BUFFER_SIZE 512
//hello
//pin set
//Serial,I2C
I2C i2c(p9, p10);        // sda, scl
Serial pc(USBTX, USBRX); // tx, rx
Serial gps(p28, p27);
Serial xbee(p13,p14);

//motor
DigitalOut motor1_A(p21);
DigitalOut motor1_B(p22);
PwmOut motor1_pwm(p23);

DigitalOut motor2_A(p24);
DigitalOut motor2_B(p25);
PwmOut motor2_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;

float target_palse = 0.0;
float pwm;

long encorder_count = 0;

//GPS
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;
    
    
    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;
        target_palse = 5.0;
        //motor2_A = 1;
        //motor2_B = 0;
        }
    else if(val == 's'){
        motor1_A = 0;
        motor1_B = 0;
        target_palse = 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 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.baud(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);
    pc.printf("start\n");
    axis.attach(&axisRenovation, 0.1);
    
    motor1_pwm = 0.0;
    //motor2_pwm = 0.3;        

    
    while (1) {

    }
}