File content as of revision 4:9ba47e5db1e2:

#include "mbed.h"
#include "functions.h"

const int addr = 0x68 << 1;
const int Sampling = 3000;

I2C i2c(PB_11 , PB_10);//PB_11 -> PB_9
                       // -> PB_8
                         //PF_0,PF_1
Timer timer;
Serial ss(USBTX,USBRX);
int elapsed_time;
char funcd[2];
double offset_gz = 0; 
int error_c = 0;

void readGyz(double* gz){
    int16_t a = i2c_read_16bit(addr,0x48,0x47);
    //ss.printf("%d\n",a);
    *gz = a*0.5;
    //0.01526717
    *gz *= 0.01527;
    elapsed_time = timer.read_ms();
    timer.reset();
    }

void mpu_setup(){
    i2c.frequency(400000);
    timer.start();
    i2c_write(addr,0x6b,0x00);
    i2c_write(addr,0x37,0x02);
    i2c_write(addr,0x27,0b1000);
    }

void get_offset(){
    offset_gz = 0;
    for(int i =0;i<Sampling;i++){
        
        double yaw;
        readGyz(&yaw);
        if(yaw == 139.323480)error_c++;
        //ss.printf("%f\n",yaw);
        offset_gz += yaw;
        }
        offset_gz /= Sampling;
    }

int rez(){
    return(error_c);
    }
    
void to_signed(double* all){
        if(*all < 0){
          *all += 360;
        }
        if(*all >= 360){
          *all -= 360;
        }
        
        if(*all >= 180){
          *all -= 360;
        }
    }

char *puc(double all){
    int a = int(all);
    if(a > 999){
        char st[4];
        int b = (int)a/1000;
        int c = int((a-b*1000)/100);
        int d = int((a-b*1000-c*100)/10);
        int e = a-b*1000-c*100-d*10;
        st[0] = ('0'+b);
        st[1] = ('0'+c);
        st[2] = ('0'+d);
        st[3] = ('0'+e);
        return(st);
    }else if(a > 99){//3桁
        char st[3];
        int b = (int)a/100;
        int c = int((a-b*100)/10);
        int d = int(a-b*100-c*10);
        st[0] = ('0'+b);
        st[1] = ('0'+c);
        st[2] = ('0'+d);
        return(st);
    }else if(a > 9){
        char st[2];
        int b = (int)a/10;
        int c = int(a-b*10);
        st[0] ='0'+b;
        st[1] ='0'+c;
        return(st);
    }else{
        char st[1];
        st[0] = '0'+a;
        return(st);
        }
    }
void i2c_write(int addr,char regist,char data)
{   
    funcd[0]=regist;
    funcd[1]=data;
    i2c.write(addr,funcd,2);
}
 
char i2c_read(int addr,char regist)
{
    funcd[0]=regist;
    i2c.write(addr,funcd,1);
    i2c.read(addr,funcd,1);
    return funcd[0];
}
 
int i2c_read_16bit(int addr,char registL,char registH)
{
    return (int(i2c_read(addr,registH))<<8)+int(i2c_read(addr,registL));
}

void offset_adjust(double* all,double* gz){
    if(!(*gz-offset_gz < 1 && *gz-offset_gz > -1)){
            *all += (*gz-offset_gz)*elapsed_time*0.001;
    }
}

float angle_adjust(float Gein,double all,float Min_ang,float Min_pwm,float Max_pwm_roll){
        float pwm;
        if(all > 0){
            pwm = all*Gein;
        }else{
            pwm = -all*Gein;
                }
        if(all < Min_ang && all > -Min_ang){
            pwm = 0;
        }
        if(pwm < Min_pwm)pwm = 0;
        if(pwm > Max_pwm_roll){
            pwm = Max_pwm_roll;
            }
    return(pwm);
    }