Takushima Yukimasa
/
R19_ManuTakukAHWGeo
a
Diff: main.cpp
- Revision:
- 4:236e5c58e8ee
- Parent:
- 3:1063aa133b62
- Child:
- 5:fcc79e507610
diff -r 1063aa133b62 -r 236e5c58e8ee main.cpp --- a/main.cpp Fri Aug 23 06:27:19 2019 +0000 +++ b/main.cpp Fri Aug 23 09:20:04 2019 +0000 @@ -48,13 +48,9 @@ {1,0,0}, // タオルの中央 }; /* 関数宣言 ------------------------------------------------------------------*/ -void flip1(void); void mecanum_Move(void); void mecanum_Turn(void); void mecanum_Stop(void); -//int sum_deg(double deg,double old); -void big_sort(int16_t data[], uint8_t num); -int median_filter(int16_t data[], int16_t add_data, const uint8_t num); /* 変数宣言 ------------------------------------------------------------------*/ //かご回転用 @@ -66,18 +62,21 @@ //タオル引く用 int pull=0, pull_f=0; -//タオル掴むよう -int grab=0, grab_f=0; +//タオル掴む用 +//sgrab_f: 中央のリミットでつかむ +int grab=0, grab_f=0, sgrab_f=0; +//タオル横展開用 int unfold_f=0; //90度回転のフラグ -int left_f=0,right_f=0; +int left_f=0, right_f=0; //y_deg: 起動時から絶対の角度 //yaw: 相対角 初期状態が0になるようにする //offset: 初期状態を0にするために引く値 -double y_deg, yaw, offset; +double y_deg, offset; +int yaw=0; //Turn_val: 補正の目標値 //dif_val: 補正値との差 @@ -88,6 +87,9 @@ //short_lim: 腕手前側 int short_lim, max_lim, start_lim; +//機体の前方を変更する +int dir=0; + int limit_up,limit_down; // リミットスイッチ int slide_start,slide_stop; //かごのリミット @@ -126,8 +128,10 @@ for(int i=0; i<7; i++) i2c.In(In_Data,i); - y_deg = (double)mpu.ypr[0] * 180 / M_PI; - yaw = offset-y_deg; +// y_deg = (double)mpu.ypr[0] * 180 / M_PI; +// yaw = offset-y_deg; + yaw=90*dir; + yaw%=360; dif_val = y_deg-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); @@ -227,15 +231,15 @@ } //タオルつかむ 四角 - if(grab_f) { + if(grab_f || sgrab_f) { switch(grab) { //腕を前に動かす case 0: //爪開く i2c.Out_Set(Out_Data,1,1); MD_SET_DRIVE(MD_Data,4,MD_REVERSE); - MD_SET_PWM(MD_Data,4,60); - if(max_lim==1) { + MD_SET_PWM(MD_Data,4,40); + if((sgrab_f && short_lim) || max_lim==1) { grab++; tim.reset(); } @@ -275,7 +279,10 @@ case 4: MD_SET_DRIVE(MD_Data,4,MD_FORWARD); MD_SET_PWM (MD_Data,4,60); - if(start_lim==1) grab_f=0; + if(start_lim==1) { + grab_f=0; + sgrab_f=0; + } break; } } //if(grab_f) @@ -413,7 +420,8 @@ // 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("deg:%f dif:%f ",y_deg,dif_val); +// pc.printf("%2d ",dir); pc.printf("\n\r"); @@ -439,14 +447,16 @@ //90度回転して補正の角度を固定する if(LEFTkey) { if(once==0) { - left_f++; - left_f%=2; + dir++; +// left_f++; +// left_f%=2; once++; } } else if(RIGHTkey) { if(once==0) { - right_f++; - right_f%=2; + dir--; +// right_f++; +// right_f%=2; once++; } } @@ -465,6 +475,12 @@ grab_f%=2; once++; } + } else if(TRIANGLE) { + if(once==0) { + sgrab_f++; + sgrab_f%=2; + once++; + } } //倒すを引いて動かす else if(CIRCLE) {