Takushima Yukimasa
/
R19_ManuTakukAHWGeo
a
Diff: main.cpp
- Revision:
- 5:fcc79e507610
- Parent:
- 4:236e5c58e8ee
- Child:
- 6:040d001acb12
diff -r 236e5c58e8ee -r fcc79e507610 main.cpp --- a/main.cpp Fri Aug 23 09:20:04 2019 +0000 +++ b/main.cpp Sat Aug 24 00:30:20 2019 +0000 @@ -75,8 +75,10 @@ //y_deg: 起動時から絶対の角度 //yaw: 相対角 初期状態が0になるようにする //offset: 初期状態を0にするために引く値 -double y_deg, offset; -int yaw=0; +double y_deg[2], offset; +double yaw=0; +double sum=0, ave=0; +int offset_cnt=0; //Turn_val: 補正の目標値 //dif_val: 補正値との差 @@ -120,19 +122,33 @@ int main() { //flipper1.attach(&flip1, 0.001); // 割り込み + while(1) { //自動系のタイマースタート tim.start(); mpu.loop(); - if(sbdbt.State_check()) { - for(int i=0; i<7; i++) i2c.In(In_Data,i); + y_deg[1] = y_deg[0]; + y_deg[0] = (double)mpu.ypr[0] * 180 / M_PI; + + //180~-180をまたいだ時 + if(y_deg[1]<-90 && y_deg[0]>90) y_deg[1]+=360; + else if(y_deg[1]>90 && y_deg[0]<-90) y_deg[1]-=360; -// y_deg = (double)mpu.ypr[0] * 180 / M_PI; -// yaw = offset-y_deg; - yaw=90*dir; - yaw%=360; - dif_val = y_deg-Turn_val; + if(sbdbt.State_check()) { + for(int i=0; i<7; i++) i2c.In(In_Data,i); + + //誤差をさんぷりんぐ + if(offset_cnt<1000) { + if(offset_cnt!=0) { + sum+=(y_deg[0]-y_deg[1]); + ave=sum/offset_cnt; + } + offset_cnt++; + } else yaw += (y_deg[0]-y_deg[1])-ave; +// yaw=90*dir; +// yaw%=360; + dif_val = y_deg[0]-Turn_val; lx = ((64-sbdbt.LY)*100.0/64)*sin(yaw*M_PI/180)+((sbdbt.LX-64)*100.0/64)*cos(yaw*M_PI/180); ly = ((64-sbdbt.LY)*100.0/64)*cos(yaw*M_PI/180)-((sbdbt.LX-64)*100.0/64)*sin(yaw*M_PI/180); @@ -176,7 +192,7 @@ } // if(g) }// if(LX !=64 || LY !=64) else if(sbdbt.RX != 64) { // 旋回 - Turn_val=y_deg; + Turn_val=y_deg[0]; mecanum_Turn(); } else { mecanum_Stop(); @@ -191,7 +207,7 @@ } } else { left_f=0; - Turn_val=y_deg; + Turn_val=y_deg[0]; mecanum_Stop(); } } @@ -203,7 +219,7 @@ } } else { right_f=0; - Turn_val=y_deg; + Turn_val=y_deg[0]; mecanum_Stop(); } } @@ -420,7 +436,7 @@ // pc.printf("pull %2d%2d ",pull,pull_f); // pc.printf("grab %2d%2d ",grab,grab_f); // pc.printf("kago %2d%2d ",kago,kago_f); -// pc.printf("deg:%f dif:%f ",y_deg,dif_val); + pc.printf("0:%f 1:%f ave:%f yaw:%f",y_deg[0],y_deg[1],ave,yaw); // pc.printf("%2d ",dir); pc.printf("\n\r"); @@ -441,8 +457,8 @@ /* ----------------ボタン系--------------- */ //補正角リセット if(sbdbt.L1) { - Turn_val=y_deg; - offset=y_deg; + Turn_val=y_deg[0]; + offset=y_deg[0]; } //90度回転して補正の角度を固定する if(LEFTkey) {