Takushima Yukimasa
/
R19_ManuTakukAHWGeo
a
Diff: main.cpp
- Revision:
- 13:48c4a4c95e77
- Parent:
- 12:7e33e400a48d
- Child:
- 14:e6d16095ffa8
diff -r 7e33e400a48d -r 48c4a4c95e77 main.cpp --- a/main.cpp Sun Sep 08 09:57:22 2019 +0000 +++ b/main.cpp Sun Sep 15 11:43:13 2019 +0000 @@ -2,7 +2,7 @@ /* 型定義 --------------------------------------------------------------------*/ //ゾーン対応 -//#define _RED_ZONE_ +#define _RED_ZONE_ #ifdef _RED_ZONE_ const int def_val=0; #else @@ -18,11 +18,12 @@ //kogo: かごをひっくり返す //pull: 奥のリミットで引く -//spull: 中央のリミットでつかむ(short_pull) +//spull: 中央のリミットでつかむ(short_pull //grab: 掴む //back_f: 真ん中目で下がるとき //release_f: 全体のエアー開放用 //unfold_f: タオル横展開用 + 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; @@ -78,6 +79,7 @@ else if(rawyaw[1]>270 && rawyaw[0]<90) rawyaw[1]-=360; yaw-=rawyaw[0]-rawyaw[1]; + yaw=fmod(yaw, 360); //ゾーン対応 #ifdef _RED_ZONE_ @@ -95,7 +97,7 @@ rx = (sbdbt.RX - 64)*60/64; //R1で停止 - if(sbdbt.R1==0) { + if(1) { //メカナム基本動作 mecanum_Move(lx, ly, rx); @@ -115,7 +117,6 @@ 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); @@ -124,7 +125,6 @@ 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); @@ -147,6 +147,8 @@ 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); @@ -206,6 +208,11 @@ 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); @@ -241,7 +248,7 @@ case 3: //腕上げる - i2c.Out_Set(Out_Data,0,0); +// i2c.Out_Set(Out_Data,0,0); if(tim.read_ms()>0) { spull++; tim.reset(); @@ -269,6 +276,8 @@ 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); @@ -315,12 +324,15 @@ else if(kago_f) { // PALETTE(MAGENTA); short_f=1; - //腕の先を閉じておく - i2c.Out_Set(Out_Data,1,0); 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) { @@ -370,8 +382,11 @@ } else { //爪開く i2c.Out_Set(Out_Data,1,1); - //腕上げる - i2c.Out_Set(Out_Data,0,0); + + //腕おろす + if(sbdbt.R1) i2c.Out_Set(Out_Data,0,1); + //腕あげる + else i2c.Out_Set(Out_Data,0,0); //腕真ん中で停止 if(short_f) { @@ -385,9 +400,32 @@ 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); + else MD_SET_DRIVE(MD_Data,4,MD_REVERSE); MD_SET_PWM (MD_Data,4,30); - } else { + } + //赤ゾーン + else if(yaw<=45 || 315<=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(135<=yaw && yaw<=225) { + 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); + } + } + + //止まる + else { MD_SET_DRIVE(MD_Data,4,MD_BRAKE); MD_SET_PWM (MD_Data,4,0); } @@ -445,16 +483,6 @@ slide_start = (In_Data+3)->in_data; slide_stop = (In_Data+0)->in_data; - //半自動のデバック -// 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("%d %d",keep_tim[0].read_ms(),keep_f[0]); - pc.printf("\n\r"); - /* ----------------ボタン系--------------- */ //角度リセット if(sbdbt.L1) { @@ -510,7 +538,17 @@ } // if(sbdbt.State_check()) - else PALETTE(BLACK); + + //半自動のデバック +// 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("%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); + pc.printf("\n\r"); } // while(1) } // int main()