Ryu Kaz
/
767zi
t6est
functions.cpp
- Committer:
- kazuryu
- Date:
- 2019-09-24
- Revision:
- 0:3dc012104243
- Child:
- 4:9ba47e5db1e2
File content as of revision 0:3dc012104243:
#include "mbed.h" #include "functions.h" const int addr = 0x68 << 1; const int Sampling = 4000; I2C i2c(PB_11 , PB_10);//PB_11 -> PB_9 //PB_10 -> 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(){ 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); }