File content as of revision 4:341ad97bf8a6:

#include "mbed.h"
#include <stdlib.h>
#include "MPU6050.h"
#include "QEI.h"
#include "TB6612FNG.h"

#define MPU6050_WHO_AM_I     0x75  // Read Only
#define MPU6050_PWR_MGMT_1   0x6B  // Read and Write
#define MPU_ADDRESS  0x68
#define MPU_ADDRESS2  0x69
#define TYP1 16384.0 //
#define TYP2 8192.0 //
#define TYP3 4096.0 //
#define TYP4 2048.0 //
#define period 4000
#define rate 128
int t_1, t_2, t_3, t_4;
int t_21, t_22, t_23;
int t_11, t_12, t_13;
int i = rate;
int j;
bool flug = true;
unsigned long tm, tm2, tm3 = 0;
int16_t ax,ay,az;
//float ax2,ay2,az2;
float Role = 0;
int num;
char ch;
float w = 0; 
float dw = 0.1;
Timer t;
//初期設定
//加速度センサ
MPU6050 mpu1(p9,p10,0x68);
MPU6050 mpu2(p9,p10,0x69);
Serial pc(USBTX,USBRX);
//pc.baud(115200);
//pc.baud(9600);
//pc.baud(230400);
//pc.baud(200000);
char data2[6];
char data3[6];
//シリアル
//Serial pc(USBTX,USBRX);
//エンコーダ
QEI wheel(p22, p21, NC, 6, QEI::X2_ENCODING);
//タイマー
Ticker flipper;
void flip() {
        t_1 = t.read_us();
        pc.printf("%d\n", t_1 - t_2);
        if (mpu1.read_data(mpu1.ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6)){
            t_3 = t.read_us();
            pc.printf("%d\n", t_3 - t_2);
            if (mpu2.read_data(mpu2.ADDRESS, MPU6050_ACCEL_XOUT_H, data3, 6)) {
                t_4 = t.read_us();
                pc.printf("%d\n", t_4 - t_2);
                pc.printf("%d\n", int(data2[0] << 8 | data2[1]));
                pc.printf("%d\n", int(data2[2] << 8 | data2[3]));
                pc.printf("%d\n", int(data2[4] << 8 | data2[5]));
                pc.printf("%d\n", int(data3[0] << 8 | data3[1]));
                pc.printf("%d\n", int(data3[2] << 8 | data3[3]));
                pc.printf("%d\n", int(data3[4] << 8 | data3[5]));
                t_2 = t_1;
                i--;
            }
        }
        if(!i){
            flipper.detach();
            t_1 = t.read_us();
            Role = ((float)wheel.getPulses())*1000000 / (6*t_1);
            pc.printf("%f\n", Role);
            flug = true;
            i = rate;
        }
}


int main(){
    TB6612FNG motor(p18, p19, p20, p26);
    //pc.baud(115200);
//pc.baud(9600);
    pc.baud(230400);
//pc.baud(200000);

    mpu1.start();
    mpu2.start();
    //mpu1.start();
    //mpu2.start();
    motor.STBY(1);
    motor.Forward();
    //pc.printf("Hello3\n");
    while(1){
        if(flug){
            if(pc.readable()) {
                char ch[5];    // 受信確認
                while(pc.readable()){
                    if(j == 0)wait(0.1);
                    //pc.printf("%d\n", j);
                    ch[j] = pc.getc();
                    j++;
                    //wait(0.01);
                }    // 1文字取り出し
                ch[j] = '\0';
                num = atoi(ch);
                j = 0;
                switch(num){
                    case -1:
                        flug = false;
                        t_2 = 0;
                        t.reset();
                        wheel.reset();
                        t.start();
                        flipper.attach(&flip, 0.1);
                        break;
                    case -2:
                        motor.Stop();
                        break;
                    case -3:
                        motor.Forward();
                        break;
                    case -4:
                        break;
                    default: 
                        motor.SetW(float(num)/255);
                        break;
                }  
            }
        }
    }
}