File content as of revision 0:cb965cafb758:

#include "mbed.h"
#define pi 3.141592653589794
#define waddr 0xD0
#define raddr 0xD1
#define kt 0.1  //出力値の各ゲインを調節
#define ko 0.1
#define kp 0.1
#define kd 0.1
PwmOut mp[4] ={p21,p22,p23,p24};
I2C sen(p9,p10);
Timer global;
Timer pt;
void motor_cont(char select,double percent)//selectが0の時は左がselectが１の時は右のモータをコントロール。
{
    select *= 2;
    if (percent>0){
        mp[select] = fabs(percent);
        mp[select+1] = 0;
    }else if (percent <0){
        mp[select] = 0;
        mp[select+1] = fabs(percent);
    }else{
        mp[select] = 0;
        mp[select+1] = 0;
    }
}
void sensor_init(){ //センサ計測よーーーーーい
     sen.frequency(400000);
     char put[2][2]; //送る値の用意
     put[0][0] = 0x6b;
     put[0][1] = 0x00;
     put[1][0] = 0x37;
     put[1][1] = 0x02;
     sen.write(waddr,put[0],2); //送信
     sen.write(waddr,put[1],2);
}
void get_gyro(double *x,double *y,double *z,int *time) //ジャイロの値をもってくる
{
    int i; //変数用意
    char tv[6];
    short t[3];
    char put[] ={0x43};
    sen.write(waddr,put,1); //センサに送信
    sen.read(raddr,tv,6);
    for(i = 0;i <3;i++){ //xyzそれぞれの値を算出
        t[i] = tv[2*i]<<8+tv[2*i+1];
    }
    global.stop();
    *time = global.read_us();
    global.reset();
    global.start();
    *x = t[0]*0.00763; //dag/secに単位換算
    *y = t[1]*0.00763;
    *z = t[2]*0.00763;
}
    
//------------------main function------------------------------------------------------------------------------------------
int main() {
    global.start(); //プログラムでの経過時間をけいそく（積分などで利用）
    sensor_init();
    double x,y,z,x2,theta,omega,power,power2,distance;
    int time,time2;
    while(1) {
        get_gyro(&x,&y,&z,&time);
        theta = theta+(x+x2)*time/(2.0*1000000.0); //積分して角度算出
        omega = x;
        power = kt*theta + ko*omega +kp*power;
        time2 = pt.read_us();
        distance = distance + (power+power2)*time2/(2.0*1000000.0); //移動距離を出力を積分して算出
        power += kd*distance;
        pt.reset();
        pt.start();
        x2 = x;
        power2 = power;
    }
}