Takushima Yukimasa
/
R19_ManuTakukAHWGeo
a
Diff: main.cpp
- Revision:
- 16:829b953d1ac1
- Parent:
- 15:68720ff6bbc9
- Child:
- 17:446be2c278d1
diff -r 68720ff6bbc9 -r 829b953d1ac1 main.cpp --- a/main.cpp Mon Sep 23 06:10:11 2019 +0000 +++ b/main.cpp Mon Sep 23 08:45:17 2019 +0000 @@ -61,17 +61,15 @@ /*----------------------------------- 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; + yaw=def_val; //初期角度 + Turn_val=def_val; //補正角度 sbdbt.LX=64; sbdbt.LY=64; @@ -104,6 +102,8 @@ if(zone==0) PALETTE(RED); else PALETTE(BLUE); + if(release_f) PALETTE(YELLOW); + //リミット for(int i=0; i<7; i++) i2c.In(In_Data,i); @@ -111,55 +111,302 @@ 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); + //メカナム基本動作 + 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(); + + 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(); - //旋回している間タイマーをリセット - if(sbdbt.RX != 64) drift_tim.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); + } - //旋回して慣性で動いた後の角度に補正する - if(drift_tim.read_ms()<1000) Turn_val=yaw; + //タオルつかむ 四角 + 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; + + case 1: + //腕停止 + MD_SET_DRIVE(MD_Data,4,MD_BRAKE); + MD_SET_PWM(MD_Data,4,0); - //角度補正 - AngleCorrection(yaw, Turn_val); + //腕おろす + 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(!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(); + //腕戻す + 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++; + tim.reset(); + } + break; - MD_SET_DRIVE(MD_Data, 5,MD_FORWARD); - MD_SET_PWM(MD_Data, 5,100); + case 2: + //爪閉じる + i2c.Out_Set(Out_Data,1,0); + if(tim.read_ms()>0) { + spull++; + 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; + } - //下方向 - 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(); + } //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; - 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(); + 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) { + short_f=1; + + 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_PWM(MD_Data,5,0); - MD_SET_DRIVE(MD_Data, 5,MD_BRAKE); + //かご回転 + 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; } + } 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: //爪開く @@ -171,7 +418,7 @@ MD_SET_PWM (MD_Data,4,60); if(max_lim==1) { - grab++; + disturb++; tim.reset(); } break; @@ -184,72 +431,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; @@ -258,314 +440,105 @@ //爪閉じる 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(); - } - 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; - - 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; - - case 2: - if(tim.read_ms()>0) { - kago++; - 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 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(); + //青ゾーン + 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; - } - } 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(disturb_f) { - //真ん中で止まる用 - short_f=1; - switch(disturb) { - //腕を前に動かす - 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) { - disturb++; - tim.reset(); - } - break; - - case 1: - //腕停止 - MD_SET_DRIVE(MD_Data,4,MD_BRAKE); - MD_SET_PWM(MD_Data,4,0); + } //if(disturb_f) - //腕おろす - i2c.Out_Set(Out_Data,0,1); - if(tim.read_ms()>200) { - disturb++; - tim.reset(); - } - break; - - case 2: - //爪閉じる - i2c.Out_Set(Out_Data,1,0); - if(tim.read_ms()>0) { - disturb++; - tim.reset(); - } - break; - - case 3: - //腕上げる - i2c.Out_Set(Out_Data,0,0); - - //赤ゾーン - 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); - } - } - //青ゾーン - 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); - } - } - } - } //if(disturb_f) + //腕真ん中で停止 + else if(short_f) { + //爪開く + i2c.Out_Set(Out_Data,1,1); - //腕真ん中で停止 - else if(short_f) { - //爪開く - i2c.Out_Set(Out_Data,1,1); - - 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 { - //爪開く - i2c.Out_Set(Out_Data,1,1); + 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( 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); - } - } - //青ゾーン - 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); - } - } + 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( 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); } - //かご停止 - MD_SET_DRIVE(MD_Data,6,MD_BRAKE); - MD_SET_PWM (MD_Data,6,0); + } + //青ゾーン + 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); + } + } - kago=0; - pull=0; - grab=0; - spull=0; + //止まる + else { + MD_SET_DRIVE(MD_Data,4,MD_BRAKE); + MD_SET_PWM (MD_Data,4,0); } - } - } - //非常停止 - else { - for(int i=0; i<8; i++) { - MD_SET_DRIVE(MD_Data,i,MD_BRAKE); - MD_SET_PWM (MD_Data,i,0); + //かご停止 + MD_SET_DRIVE(MD_Data,6,MD_BRAKE); + MD_SET_PWM (MD_Data,6,0); + + kago=0; + pull=0; + grab=0; + spull=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); + i2c.Out_Set(Out_Data, 2, sbdbt.L2); //タオル掛け + i2c.Out_Set(Out_Data, 4, sbdbt.R2); //タオルを引っ張るやつ + i2c.Out_Set(Out_Data, 3, unfold_f); //タオル展開 + i2c.Out_Set(Out_Data, 5, release_f);//エアー解放 - //エアー解放 - 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); + //-------------- 出力 --------------/ + 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; @@ -582,71 +555,17 @@ yaw=def_val; Turn_val=def_val; } - //かごを倒す - 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++; - } - } - //倒すを引いて動かす - 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.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; + if(CROSS) Toggle(&kago_f, CROSS); //かごを倒す + else if(SQUARE) Toggle(&grab_f, SQUARE); //タオルを掴んで動かす + else if(TRIANGLE) Toggle(&spull_f, TRIANGLE); + else if(CIRCLE) Toggle(&pull_f, CIRCLE); + else if(sbdbt.START) Toggle(&unfold_f, sbdbt.START); + else if(sbdbt.SELECT) Toggle(&release_f, sbdbt.SELECT); + else if(sbdbt.PS) Toggle(&disturb_f, sbdbt.PS); + 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("%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("%d %d ",kago_f, grab_f); pc.printf("\n\r"); } // while(1) } // int main() @@ -687,7 +606,6 @@ void Toggle(int *variable, int val) { - static int once=0; static int data[2]= {0}; if(once==0) { @@ -695,39 +613,11 @@ once=1; } - //値の更新 data[1]=data[0]; data[0]=val; if(data[1]==0 && data[0]==1) { - *variable++; - *variable%=2; - } else if(data[1]==1 && data[0]==0) once=0; -} - -/*操作法*/ -/* -〇 ボタン == 回収(引きずる) -? ボタン == 籠 -△ ボタン == 回収(シーツ兼シャツ) -□ ボタン == 回収(タオル) -上 ボタン == ハンガーかけ機構 上昇 -下 ボタン == ハンガーかけ機構 下降 -L1 ボタン == リセット(足回り) -L2 ボタン == タオル掛け -R2 ボタン == タオル真ん中掴む -select == ハンガー掛け 最大まで上昇 -start == ハンガー展開 -*/ - -void Toggle(int *variable, int val){ - static int data[2]={0}; - - data[1]=data[0]; - data[0]=val; - - if(data[1]==0 && data[0]==1){ - *variable++; - *variable%=2; + (*variable)++; + (*variable)%=2; } } \ No newline at end of file