File content as of revision 2:4a512a6e6b1d:

#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 100000
#define rate 32
 int t_1, t_2;
int i = -1;
int j;
bool flug = false;
unsigned long tm, tm2, tm3 = 0;
int ax,ay,az;
//float ax2,ay2,az2;
float Role = 0;
int num;
char ch;
float w = 0; 
float dw = 0.1;

//初期設定
//加速度センサ
//MPU6050 mpu1(p9,p10,0x68);
//MPU6050 mpu2(p9,p10,0x69);
//シリアル
Serial pc(USBTX,USBRX);
//エンコーダ
QEI wheel(p22, p21, NC, 6, QEI::X2_ENCODING);
//タイマー
Ticker flipper;
void flip() {
    flug = true;
    //pc.printf("AA");
}
Timer t;
//モータ


int main(){
    TB6612FNG motor(p18, p19, p20, p26);
    ax = 0;
    ay = 1;
    az = 3;
    Serial pc(USBTX,USBRX);
    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(i == -1){
            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:
                        i = 0;
                        flipper.attach_us(&flip, period);
                        break;
                    case -2:
                        motor.Stop();
                        break;
                    case -3:
                        motor.Forward();
                        break;
                    case -4:
                        break;
                    default: 
                        motor.SetW(float(num)/255);
                        break;
                }  
            }
        }
              //      t.reset();
              //      t.start();
        if(flug){
            //tm = t.read_us();
            //tm2 = tm - tm3;
            //tm3 = tm;
            flug = false;
            if(!i){
                t_2 = 0;
                t.reset();
                wheel.reset();
                t.start();
            }
            //pc.printf("Hello\n");
            //mpu1.read(&gx2,&gy2,&gz2,&ax2,&ay2,&az2);
            //pc.printf("%f\n%f\n%f\n",ax2,ay2,az2);
            //mpu1.readraw2(&ax,&ay,&az);
            pc.printf("%d\n%d\n%d\n",ax,ay,az);
            //mpu2.read(&gx2,&gy2,&gz2,&ax2,&ay2,&az2);
            //pc.printf("%f\n%f\n%f\n",ax2,ay2,az2);
            //mpu2.readraw2(&ax,&ay,&az);
            pc.printf("%d\n%d\n%d\n",ax,ay,az);
            t_1 = t.read_us();
            pc.printf("%d\n", t_1 - t_2);
            t_2 = t_1;
            i++;
            if(i >= rate){
                flipper.detach();
                t_1 = t.read_us();
                Role = ((float)wheel.getPulses())*1000000 / (6*t_1);
                pc.printf("%f\n", Role);
                i = -1;
            }
        }
    }
}