Takushima Yukimasa
/
R19_ManuTakukAHWGeo
a
Diff: main.cpp
- Revision:
- 14:e6d16095ffa8
- Parent:
- 13:48c4a4c95e77
- Child:
- 15:68720ff6bbc9
- Child:
- 18:d6be6154f9aa
diff -r 48c4a4c95e77 -r e6d16095ffa8 main.cpp --- a/main.cpp Sun Sep 15 11:43:13 2019 +0000 +++ b/main.cpp Sat Sep 21 01:06:15 2019 +0000 @@ -1,18 +1,18 @@ #include "main.h" /* 型定義 --------------------------------------------------------------------*/ -//ゾーン対応 -#define _RED_ZONE_ -#ifdef _RED_ZONE_ -const int def_val=0; -#else -const int def_val=180; -#endif /* 関数宣言 ------------------------------------------------------------------*/ /* 変数宣言 ------------------------------------------------------------------*/ +//0: 赤 +//1: 青 +int zone=0; + +//デフォルトの角度 +int def_val=0; + //コントローラーの状態を保存するため int once=0; @@ -27,20 +27,23 @@ int kago=0, kago_f=0, pull=0, pull_f=0, grab=0, grab_f=0, spull=0, spull_f=0; int back_f=0, release_f=0, unfold_f=0, short_f=0; +//妨害に使う +int disturb, disturb_f; + //上下動作を保持 //[0]: 上 //[1]: 下 int keep_f[2]= {0}; -//yaw: 実際に使う角度 -double yaw=def_val; +//実際に使う角度 +double yaw; //[0]: 今回の値 //[1]: 前回の値 double rawyaw[2]; //Turn_val: 補正の目標値 -double Turn_val=def_val; +double Turn_val; //short_lim: 腕中央 //max_lim: 腕奥側 @@ -58,6 +61,22 @@ /*----------------------------------- main -----------------------------------*/ int main() { + //リミット + i2c.In(In_Data,7); + zone=(In_Data+7)->in_data; + + if(zone==0) def_val=0; + else def_val=180; + + yaw=def_val; + + //補正角度 + Turn_val=def_val; + + sbdbt.LX=64; + sbdbt.LY=64; + sbdbt.RX=64; + sbdbt.RY=64; //ジャイロリセット bno.reset(); @@ -79,70 +98,318 @@ else if(rawyaw[1]>270 && rawyaw[0]<90) rawyaw[1]-=360; yaw-=rawyaw[0]-rawyaw[1]; - yaw=fmod(yaw, 360); + yaw=fmod(yaw, 360.0); //ゾーン対応 -#ifdef _RED_ZONE_ - PALETTE(RED); -#else - PALETTE(BLUE); -#endif + if(zone==0) PALETTE(RED); + else PALETTE(BLUE); + + //リミット + for(int i=0; i<7; i++) i2c.In(In_Data,i); + + 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); + rx = (sbdbt.RX - 64)*60/64; + + //R1で停止 + if(1) { + //メカナム基本動作 + mecanum_Move(lx, ly, rx); + + //旋回している間タイマーをリセット + if(sbdbt.RX != 64) drift_tim.reset(); + + //旋回して慣性で動いた後の角度に補正する + if(drift_tim.read_ms()<1000) Turn_val=yaw; + + //角度補正 + AngleCorrection(yaw, Turn_val); + + /*---ハンガーかけるやつ----------------------------------------------*/ + //上方向 + if(!DOWNkey && ((UPkey || keep_f[0]) && limit_up == 0)) { + //一定秒以上押されたら保持 + if(keep_tim[0].read_ms()>500) keep_f[0]=1; + //下方向の保持を解除 + keep_f[1]=0; + keep_tim[1].reset(); - if(sbdbt.State_check()) { - //リミット - for(int i=0; i<7; i++) i2c.In(In_Data,i); + MD_SET_DRIVE(MD_Data, 5,MD_FORWARD); + MD_SET_PWM(MD_Data, 5,100); + } + //下方向 + else if(!UPkey && ((DOWNkey || keep_f[1]) && limit_down == 0)) { + if(keep_tim[1].read_ms()>500) keep_f[1]=1; + keep_f[0]=0; + keep_tim[0].reset(); + + MD_SET_DRIVE(MD_Data, 5,MD_REVERSE); + MD_SET_PWM(MD_Data, 5,100); + } else { + keep_f[0]=0; + keep_tim[0].reset(); + keep_f[1]=0; + keep_tim[1].reset(); + + MD_SET_PWM(MD_Data,5,0); + MD_SET_DRIVE(MD_Data, 5,MD_BRAKE); + } - 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); - rx = (sbdbt.RX - 64)*60/64; + //タオルつかむ 四角 + if(grab_f) { + //真ん中で止まる用 + short_f=1; + switch(grab) { + //腕を前に動かす + case 0: + //爪開く + i2c.Out_Set(Out_Data,1,1); + //腕上げる + i2c.Out_Set(Out_Data,0,0); + + MD_SET_DRIVE(MD_Data,4,MD_REVERSE); + MD_SET_PWM (MD_Data,4,60); + + if(max_lim==1) { + grab++; + tim.reset(); + } + break; - //R1で停止 - if(1) { - //メカナム基本動作 - mecanum_Move(lx, ly, rx); + case 1: + //腕停止 + MD_SET_DRIVE(MD_Data,4,MD_BRAKE); + MD_SET_PWM(MD_Data,4,0); + + //腕おろす + i2c.Out_Set(Out_Data,0,1); + if(tim.read_ms()>200) { + grab++; + tim.reset(); + } + break; + + case 2: + //爪閉じる + i2c.Out_Set(Out_Data,1,0); + if(tim.read_ms()>0) { + grab++; + tim.reset(); + } + break; + + case 3: + //腕上げる + i2c.Out_Set(Out_Data,0,0); + if(tim.read_ms()>0) { + grab++; + tim.reset(); + } + break; - //旋回している間タイマーをリセット - if(sbdbt.RX != 64) drift_tim.reset(); + //腕戻す + case 4: + MD_SET_DRIVE(MD_Data,4,MD_FORWARD); + MD_SET_PWM (MD_Data,4,60); + if(start_lim==1) { + grab_f=0; + tim.reset(); + } + break; + } + } //if(grab_f) + + //シーツ用 + else if(spull_f) { + short_f=1; + switch(spull) { + //腕を前に動かす + case 0: + //爪閉じる + i2c.Out_Set(Out_Data,1,0); + //腕上げる + i2c.Out_Set(Out_Data,0,0); - //旋回して慣性で動いた後の角度に補正する - if(drift_tim.read_ms()<1000) Turn_val=yaw; + //真ん中のリミットより前に腕があった場合にバックする + if(back_f) MD_SET_DRIVE(MD_Data,4,MD_FORWARD); + else MD_SET_DRIVE(MD_Data,4,MD_REVERSE); + MD_SET_PWM (MD_Data,4,30); + + if(short_lim==1) { + spull++; + tim.reset(); + } else if(max_lim) back_f=1; + break; - //角度補正 - AngleCorrection(yaw, Turn_val); + case 1: + //腕停止 + MD_SET_DRIVE(MD_Data,4,MD_BRAKE); + MD_SET_PWM(MD_Data,4,0); + + //腕おろす + i2c.Out_Set(Out_Data,0,1); + if(tim.read_ms()>200) { + spull++; + tim.reset(); + } + break; - /*---ハンガーかけるやつ----------------------------------------------*/ - //上方向 - if(!DOWNkey && ((UPkey || keep_f[0]) && limit_up == 0)) { - //一定秒以上押されたら保持 - if(keep_tim[0].read_ms()>500) keep_f[0]=1; - //下方向の保持を解除 - keep_f[1]=0; + case 2: + //爪閉じる + i2c.Out_Set(Out_Data,1,0); + if(tim.read_ms()>0) { + spull++; + tim.reset(); + } + break; - MD_SET_DRIVE(MD_Data, 5,MD_FORWARD); - MD_SET_PWM(MD_Data, 5,100); + case 3: + //腕上げる +// i2c.Out_Set(Out_Data,0,0); + if(tim.read_ms()>0) { + spull++; + tim.reset(); + } + break; + + //腕戻す + case 4: + MD_SET_DRIVE(MD_Data,4,MD_FORWARD); + MD_SET_PWM (MD_Data,4,60); + if(start_lim==1) { + spull_f=0; + tim.reset(); + } + break; + } - //下方向 - else if(!UPkey && ((DOWNkey || keep_f[1]) && limit_down == 0)) { - if(keep_tim[1].read_ms()>500) keep_f[1]=1; - keep_f[0]=0; + } //if(spull_f) + + //タオル引く 丸 + else if(pull_f) { + short_f=1; + switch(pull) { + //腕を前に動かす + case 0: + //爪閉じる + i2c.Out_Set(Out_Data,1,0); + //腕上げる + i2c.Out_Set(Out_Data,0,0); + + MD_SET_DRIVE(MD_Data,4,MD_REVERSE); + MD_SET_PWM (MD_Data,4,60); + + if(max_lim==1) { + pull++; + tim.reset(); + } + break; + + case 1: + //腕停止 + MD_SET_DRIVE(MD_Data,4,MD_BRAKE); + MD_SET_PWM (MD_Data,4,0); + + //腕おろす + i2c.Out_Set(Out_Data,0,1); + if(tim.read_ms()>100) { + pull++; + tim.reset(); + } + break; + + //停止 + case 2: + if(tim.read_ms()>0) { + pull++; + tim.reset(); + } + break; + + case 3: + MD_SET_DRIVE(MD_Data,4,MD_FORWARD); + MD_SET_PWM (MD_Data,4,60); + if(start_lim==1) { + pull_f=0; + tim.reset(); + } + break; + } + } //if(pull_f) + + //かごを倒す + else if(kago_f) { +// PALETTE(MAGENTA); + short_f=1; - MD_SET_DRIVE(MD_Data, 5,MD_REVERSE); - MD_SET_PWM(MD_Data, 5,100); - } else { - keep_f[0]=0; - keep_tim[0].reset(); - keep_f[1]=0; - keep_tim[1].reset(); + switch(kago) { + //腕を前に動かす + case 0: + //爪閉じる + i2c.Out_Set(Out_Data,1,0); + //腕上げる + i2c.Out_Set(Out_Data,0,0); + + MD_SET_DRIVE(MD_Data,4,MD_REVERSE); + MD_SET_PWM (MD_Data,4,60); + if(max_lim==1) { + kago++; + tim.reset(); + } + break; + + case 1: + //腕停止 + MD_SET_DRIVE(MD_Data,4,MD_BRAKE); + MD_SET_PWM (MD_Data,4,0); + + //かご回転 + MD_SET_DRIVE(MD_Data,6,MD_FORWARD); + MD_SET_PWM (MD_Data,6,100); + if(slide_stop==1) { + kago++; + tim.reset(); + MD_SET_DRIVE(MD_Data,6,MD_BRAKE); + MD_SET_PWM (MD_Data,6,0); + } + break; - MD_SET_PWM(MD_Data,5,0); - MD_SET_DRIVE(MD_Data, 5,MD_BRAKE); + case 2: + if(tim.read_ms()>0) { + kago++; + tim.reset(); + } + break; + + case 3: + MD_SET_DRIVE(MD_Data,6,MD_REVERSE); + MD_SET_PWM (MD_Data,6,100); + if(slide_start==1) kago++; + break; + + case 4: + MD_SET_DRIVE(MD_Data,4,MD_FORWARD); + MD_SET_PWM (MD_Data,4,60); + if(start_lim==1) { + kago_f=0; + tim.reset(); + } + break; } + } else { + if(sbdbt.R1) { + //腕おろす + i2c.Out_Set(Out_Data,0,1); + //爪閉じる + i2c.Out_Set(Out_Data,1,0); + } + //腕あげる + else i2c.Out_Set(Out_Data,0,0); - //タオルつかむ 四角 - if(grab_f) { + if(disturb_f) { //真ん中で止まる用 short_f=1; - switch(grab) { + switch(disturb) { //腕を前に動かす case 0: //爪開く @@ -154,7 +421,7 @@ MD_SET_PWM (MD_Data,4,60); if(max_lim==1) { - grab++; + disturb++; tim.reset(); } break; @@ -167,72 +434,7 @@ //腕おろす i2c.Out_Set(Out_Data,0,1); if(tim.read_ms()>200) { - grab++; - tim.reset(); - } - break; - - case 2: - //爪閉じる - i2c.Out_Set(Out_Data,1,0); - if(tim.read_ms()>0) { - grab++; - tim.reset(); - } - break; - - case 3: - //腕上げる - i2c.Out_Set(Out_Data,0,0); - if(tim.read_ms()>0) { - grab++; - tim.reset(); - } - break; - - //腕戻す - case 4: - MD_SET_DRIVE(MD_Data,4,MD_FORWARD); - MD_SET_PWM (MD_Data,4,60); - if(start_lim==1) { - grab_f=0; - tim.reset(); - } - break; - } - } //if(grab_f) - - //シーツ用 - else if(spull_f) { - short_f=1; - switch(spull) { - //腕を前に動かす - case 0: - //爪閉じる - i2c.Out_Set(Out_Data,1,0); - //腕上げる - i2c.Out_Set(Out_Data,0,0); - - //真ん中のリミットより前に腕があった場合にバックする - if(back_f) MD_SET_DRIVE(MD_Data,4,MD_FORWARD); - else MD_SET_DRIVE(MD_Data,4,MD_REVERSE); - MD_SET_PWM (MD_Data,4,30); - - if(short_lim==1) { - spull++; - tim.reset(); - } else if(max_lim) back_f=1; - break; - - case 1: - //腕停止 - MD_SET_DRIVE(MD_Data,4,MD_BRAKE); - MD_SET_PWM(MD_Data,4,0); - - //腕おろす - i2c.Out_Set(Out_Data,0,1); - if(tim.read_ms()>200) { - spull++; + disturb++; tim.reset(); } break; @@ -241,170 +443,61 @@ //爪閉じる i2c.Out_Set(Out_Data,1,0); if(tim.read_ms()>0) { - spull++; + disturb++; tim.reset(); } break; case 3: //腕上げる -// i2c.Out_Set(Out_Data,0,0); - if(tim.read_ms()>0) { - spull++; - tim.reset(); - } - break; - - //腕戻す - case 4: - MD_SET_DRIVE(MD_Data,4,MD_FORWARD); - MD_SET_PWM (MD_Data,4,60); - if(start_lim==1) { - spull_f=0; - tim.reset(); - } - break; - - } - } //if(spull_f) - - //タオル引く 丸 - else if(pull_f) { - short_f=1; - switch(pull) { - //腕を前に動かす - case 0: - //爪閉じる - i2c.Out_Set(Out_Data,1,0); - //腕上げる i2c.Out_Set(Out_Data,0,0); - MD_SET_DRIVE(MD_Data,4,MD_REVERSE); - MD_SET_PWM (MD_Data,4,60); - - if(max_lim==1) { - pull++; - tim.reset(); - } - break; - - case 1: - //腕停止 - MD_SET_DRIVE(MD_Data,4,MD_BRAKE); - MD_SET_PWM (MD_Data,4,0); - - //腕おろす - i2c.Out_Set(Out_Data,0,1); - if(tim.read_ms()>100) { - pull++; - tim.reset(); + //赤ゾーン + if( yaw<=90 || 270<yaw ) { + MD_SET_PWM(MD_Data,4,20); + if(RIGHTkey && max_lim==0) MD_SET_DRIVE(MD_Data,4,MD_REVERSE); + else if(LEFTkey && start_lim==0) MD_SET_DRIVE(MD_Data,4,MD_FORWARD); + else { + MD_SET_DRIVE(MD_Data,4,MD_BRAKE); + MD_SET_PWM (MD_Data,4,0); + } } - break; - - //停止 - case 2: - if(tim.read_ms()>0) { - pull++; - tim.reset(); + //青ゾーン + else if( 90<yaw && yaw<=270 ) { + MD_SET_PWM(MD_Data,4,20); + if(LEFTkey && max_lim==0) MD_SET_DRIVE(MD_Data,4,MD_REVERSE); + else if(RIGHTkey && start_lim==0) MD_SET_DRIVE(MD_Data,4,MD_FORWARD); + else { + MD_SET_DRIVE(MD_Data,4,MD_BRAKE); + MD_SET_PWM (MD_Data,4,0); + } } - break; - - case 3: - MD_SET_DRIVE(MD_Data,4,MD_FORWARD); - MD_SET_PWM (MD_Data,4,60); - if(start_lim==1) { - pull_f=0; - tim.reset(); - } - break; } - } //if(pull_f) - - //かごを倒す - else if(kago_f) { -// PALETTE(MAGENTA); - short_f=1; + } //if(disturb_f) - switch(kago) { - //腕を前に動かす - case 0: - //爪閉じる - i2c.Out_Set(Out_Data,1,0); - //腕上げる - i2c.Out_Set(Out_Data,0,0); + //腕真ん中で停止 + else if(short_f) { + //爪開く + 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) { - kago++; - tim.reset(); - } - break; - - case 1: - //腕停止 - MD_SET_DRIVE(MD_Data,4,MD_BRAKE); - MD_SET_PWM (MD_Data,4,0); + if(short_lim==1) { + short_f=0; + back_f=0; + } + //奥のリミットに当たったら逆回転 + if(max_lim==1) back_f=1; + //手前のリミットに当たったら正回転 + else if(start_lim==1) back_f=0; - //かご回転 - MD_SET_DRIVE(MD_Data,6,MD_FORWARD); - MD_SET_PWM (MD_Data,6,100); - if(slide_stop==1) { - kago++; - tim.reset(); - MD_SET_DRIVE(MD_Data,6,MD_BRAKE); - MD_SET_PWM (MD_Data,6,0); - } - break; - - case 2: - if(tim.read_ms()>0) { - kago++; - tim.reset(); - } - break; - - case 3: - MD_SET_DRIVE(MD_Data,6,MD_REVERSE); - MD_SET_PWM (MD_Data,6,100); - if(slide_start==1) kago++; - break; - - case 4: - MD_SET_DRIVE(MD_Data,4,MD_FORWARD); - MD_SET_PWM (MD_Data,4,60); - if(start_lim==1) { - kago_f=0; - tim.reset(); - } - break; - } + if(back_f) MD_SET_DRIVE(MD_Data,4,MD_FORWARD); + else MD_SET_DRIVE(MD_Data,4,MD_REVERSE); + MD_SET_PWM (MD_Data,4,30); } else { //爪開く i2c.Out_Set(Out_Data,1,1); - - //腕おろす - if(sbdbt.R1) i2c.Out_Set(Out_Data,0,1); - //腕あげる - else i2c.Out_Set(Out_Data,0,0); - //腕真ん中で停止 - if(short_f) { - if(short_lim==1) { - short_f=0; - back_f=0; - } - //奥のリミットに当たったら逆回転 - if(max_lim==1) back_f=1; - //手前のリミットに当たったら正回転 - else if(start_lim==1) back_f=0; - - if(back_f) MD_SET_DRIVE(MD_Data,4,MD_FORWARD); - else MD_SET_DRIVE(MD_Data,4,MD_REVERSE); - MD_SET_PWM (MD_Data,4,30); - } //赤ゾーン - else if(yaw<=45 || 315<=yaw) { + if( yaw<=90 || 270<yaw ) { MD_SET_PWM(MD_Data,4,20); if(RIGHTkey && max_lim==0) MD_SET_DRIVE(MD_Data,4,MD_REVERSE); else if(LEFTkey && start_lim==0) MD_SET_DRIVE(MD_Data,4,MD_FORWARD); @@ -414,7 +507,7 @@ } } //青ゾーン - else if(135<=yaw && yaw<=225) { + else if( 90<yaw && yaw<=270 ) { MD_SET_PWM(MD_Data,4,20); if(LEFTkey && max_lim==0) MD_SET_DRIVE(MD_Data,4,MD_REVERSE); else if(RIGHTkey && start_lim==0) MD_SET_DRIVE(MD_Data,4,MD_FORWARD); @@ -437,117 +530,123 @@ pull=0; grab=0; spull=0; - tim.reset(); - } - } - //非常停止 - else { - for(int i=0; i<8; i++) { - MD_SET_DRIVE(MD_Data,i,MD_BRAKE); - MD_SET_PWM (MD_Data,i,0); } } - - /* --------------電磁弁系-------------------- */ - /*タオル掛け*/ - if(sbdbt.L2)i2c.Out_Set(Out_Data,2,1); - else i2c.Out_Set(Out_Data,2,0); - - /*タオルを引っ張るやつ*/ - if(sbdbt.R2)i2c.Out_Set(Out_Data,4,1); - else i2c.Out_Set(Out_Data,4,0); - - //タオル展開 - if(unfold_f) i2c.Out_Set(Out_Data,3,1); - else i2c.Out_Set(Out_Data,3,0); - - //エアー解放 - if(release_f) { - PALETTE(YELLOW); - i2c.Out_Set(Out_Data,5,1); - } else i2c.Out_Set(Out_Data,5,0); - - //-------------- 出力 --------------// - //第二引数には使う最大の個数 - i2c.Out(Out_Data,9); - - //モータ出力 - for(int i=0; i<8; i++) i2c.MD_I2C(MD_Data,i); - - /* ----------- りみっと --------------- */ - limit_up = (In_Data+4)->in_data; - limit_down = (In_Data+5)->in_data; - max_lim = (In_Data+1)->in_data; - start_lim =(In_Data+2)->in_data; - short_lim = (In_Data+6)->in_data; - slide_start = (In_Data+3)->in_data; - slide_stop = (In_Data+0)->in_data; - - /* ----------------ボタン系--------------- */ - //角度リセット - if(sbdbt.L1) { - yaw=def_val; - Turn_val=def_val; + } + //非常停止 + else { + for(int i=0; i<8; i++) { + MD_SET_DRIVE(MD_Data,i,MD_BRAKE); + MD_SET_PWM (MD_Data,i,0); } - //かごを倒す - if(CROSS) { - if(once==0) { - kago_f++; - kago_f%=2; - once++; - } - } - //タオルを掴んで動かす - else if(SQUARE) { - if(once==0) { - grab_f++; - grab_f%=2; - once++; - } - } else if(TRIANGLE) { - if(once==0) { - spull_f++; - spull_f%=2; - once++; - } + } + + /* --------------電磁弁系-------------------- */ + /*タオル掛け*/ + if(sbdbt.L2)i2c.Out_Set(Out_Data,2,1); + else i2c.Out_Set(Out_Data,2,0); + + /*タオルを引っ張るやつ*/ + if(sbdbt.R2)i2c.Out_Set(Out_Data,4,1); + else i2c.Out_Set(Out_Data,4,0); + + //タオル展開 + if(unfold_f) i2c.Out_Set(Out_Data,3,1); + else i2c.Out_Set(Out_Data,3,0); + + //エアー解放 + if(release_f) { + PALETTE(YELLOW); + i2c.Out_Set(Out_Data,5,1); + } else i2c.Out_Set(Out_Data,5,0); + + //-------------- 出力 --------------// + //第二引数には使う最大の個数 + i2c.Out(Out_Data,9); + + //モータ出力 + for(int i=0; i<8; i++) i2c.MD_I2C(MD_Data,i); + + /* ----------- りみっと --------------- */ + limit_up = (In_Data+4)->in_data; + limit_down = (In_Data+5)->in_data; + max_lim = (In_Data+1)->in_data; + start_lim =(In_Data+2)->in_data; + short_lim = (In_Data+6)->in_data; + slide_start = (In_Data+3)->in_data; + slide_stop = (In_Data+0)->in_data; + + /* ----------------ボタン系--------------- */ + //角度リセット + if(sbdbt.L1) { + yaw=def_val; + Turn_val=def_val; + } + //かごを倒す + if(CROSS) { + if(once==0) { + kago_f++; + kago_f%=2; + once++; } - //倒すを引いて動かす - else if(CIRCLE) { - if(once==0) { - pull_f++; - pull_f%=2; - once++; - } + } + //タオルを掴んで動かす + else if(SQUARE) { + if(once==0) { + grab_f++; + grab_f%=2; + once++; + } + } else if(TRIANGLE) { + if(once==0) { + spull_f++; + spull_f%=2; + once++; + } + } + //倒すを引いて動かす + else if(CIRCLE) { + if(once==0) { + pull_f++; + pull_f%=2; + once++; } - //タオル展開用 - else if(sbdbt.START) { - if(once==0) { - unfold_f++; - unfold_f%=2; - once++; - } + } + //タオル展開用 + else if(sbdbt.START) { + if(once==0) { + unfold_f++; + unfold_f%=2; + once++; } - //タオル展開用 - else if(sbdbt.SELECT) { - if(once==0) { - release_f++; - release_f%=2; - once++; - } - } else once=0; - - - } // if(sbdbt.State_check()) + } + //タオル展開用 + else if(sbdbt.SELECT) { + if(once==0) { + release_f++; + release_f%=2; + once++; + } + } else if(sbdbt.PS) { + if(once==0) { + disturb=0; + disturb_f++; + disturb_f%=2; + once++; + } + } else once=0; //半自動のデバック // 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("%.1f %.1f ",rawyaw[0],rawyaw[1]); - pc.printf("yaw:%.1f %.1f",yaw, bno.euler.yaw); -// pc.printf("lx %d ly %d rx %d",lx, ly, rx); +// pc.printf("yaw:%.1f %.1f",yaw, bno.euler.yaw); // pc.printf("%d %d",keep_tim[0].read_ms(),keep_f[0]); // pc.printf("LX:%d, LY:%d, RX:%d, RY:%d",sbdbt.LX, sbdbt.LY, sbdbt.RX, sbdbt.RY); +//for(int i=0; i<7; i++) + pc.printf("%d ",sbdbt.PS); + pc.printf("%d ",disturb); pc.printf("\n\r"); } // while(1) } // int main()