Takushima Yukimasa
/
R19_ManuTakukAHWGeo
a
Diff: main.cpp
- Revision:
- 8:b79d21c8178b
- Parent:
- 6:040d001acb12
- Child:
- 9:64fcbcc182d9
diff -r 82dad43f2549 -r b79d21c8178b main.cpp --- a/main.cpp Sat Aug 24 06:11:03 2019 +0000 +++ b/main.cpp Tue Aug 27 08:54:15 2019 +0000 @@ -1,6 +1,13 @@ #include "main.h" /* 型定義 --------------------------------------------------------------------*/ +//ゾーン対応 +#define _RED_ZONE_ +#ifdef _RED_ZONE_ +const int def_val=0; +#else +const int def_val=180; +#endif /* 関数宣言 ------------------------------------------------------------------*/ @@ -15,15 +22,17 @@ int pull=0, pull_f=0; //タオル掴む用 -//sgrab_f: 中央のリミットでつかむ -int grab=0, grab_f=0, sgrab_f=0; +//spull_f: 中央のリミットでつかむ +//back_f: 真ん中目で下がるとき +int grab=0, grab_f=0, spull_f=0; +int back_f=0; //タオル横展開用 int unfold_f=0; //y_deg: ジャイロの生の値 //yaw: 実際に使う角度 -double y_deg[2], yaw=0;; +double y_deg[2], yaw=def_val; //ジャイロさんぷりんぐ用 double sum=0, ave=0; @@ -31,12 +40,13 @@ //Turn_val: 補正の目標値 //dif_val: 補正値との差 -double Turn_val, dif_val; +double Turn_val=def_val, dif_val; //short_lim: 腕中央 //max_lim: 腕奥側 //short_lim: 腕手前側 int short_lim, max_lim, start_lim; +int short_f=0; //足回りのpwm値 int duty[4]; @@ -73,14 +83,19 @@ //誤差をさんぷりんぐ if(offset_cnt<1000) { - PALETTE(RED); + PALETTE(YELLOW); if(offset_cnt!=0) { sum+=(y_deg[0]-y_deg[1]); ave=sum/offset_cnt; } offset_cnt++; } else { - PALETTE(GREEN); + //ゾーン対応 +#ifdef _RED_ZONE_ + PALETTE(RED); +#else + PALETTE(BLUE); +#endif yaw -= (y_deg[0]-y_deg[1])-ave; } @@ -91,219 +106,261 @@ 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)*30/64; + rx = (sbdbt.RX - 64)*60/64; + + //R1で停止 + if(sbdbt.R1==0) { + //メカナム基本動作 + mecanum_Move(lx, ly, rx); - //メカナム基本動作 - mecanum_Move(lx, ly, rx); + //旋回している間タイマーをリセット + if(sbdbt.RX != 64) drift_tim.reset(); + + //旋回して慣性で動いた後の角度に補正する + if(drift_tim.read_ms()<500) Turn_val=yaw; - //旋回している間タイマーをリセット - if(sbdbt.RX != 64) drift_tim.reset(); + //角度補正 + AngleCorrection(yaw, Turn_val); + + /*---ハンガーかけるやつ----------------------------------------------*/ + if(sbdbt.SELECT)as=1;//selectキー - //旋回して慣性で動いた後の角度に補正する - if(drift_tim.read_ms()<500) Turn_val=yaw; + if(as==1 && limit_up == 0) { + MD_SET_DRIVE(MD_Data, 5,MD_REVERSE); + MD_SET_PWM(MD_Data, 5,100); + } else if(UPkey && limit_up == 0) { + as=0; + MD_SET_DRIVE(MD_Data, 5,MD_REVERSE); + MD_SET_PWM(MD_Data, 5,100); + } else if(as==1 && limit_up == 1) { + as=0; + MD_SET_PWM(MD_Data,5,0); + MD_SET_DRIVE(MD_Data, 5,MD_BRAKE); + } else if(DOWNkey && limit_down == 0) { + MD_SET_DRIVE(MD_Data, 5,MD_FORWARD); + MD_SET_PWM(MD_Data, 5,100); + } else { + MD_SET_PWM(MD_Data,5,0); + MD_SET_DRIVE(MD_Data, 5,MD_BRAKE); + } - //角度補正 - AngleCorrection(yaw, Turn_val); + //タオルつかむ 四角 + if(grab_f || spull_f) { + PALETTE(GREEN); + short_f=1; + switch(grab) { + //腕を前に動かす + case 0: + //爪開く + i2c.Out_Set(Out_Data,1,1); - /*---ハンガーかけるやつ----------------------------------------------*/ - if(sbdbt.SELECT)as=1;//selectキー + 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,40); - if(as==1 && limit_up == 0) { - MD_SET_DRIVE(MD_Data, 5,MD_REVERSE); - MD_SET_PWM(MD_Data, 5,50); - } else if(UPkey && limit_up == 0) { - as=0; - MD_SET_DRIVE(MD_Data, 5,MD_REVERSE); - MD_SET_PWM(MD_Data, 5,50); - } else if(as==1 && limit_up == 1) { - as=0; - MD_SET_PWM(MD_Data,5,0); - MD_SET_DRIVE(MD_Data, 5,MD_BRAKE); - } else if(DOWNkey && limit_down == 0) { - MD_SET_DRIVE(MD_Data, 5,MD_FORWARD); - MD_SET_PWM(MD_Data, 5,50); - } else { - MD_SET_PWM(MD_Data,5,0); - MD_SET_DRIVE(MD_Data, 5,MD_BRAKE); - } + if(spull_f) { + if(short_lim==1) { + grab++; + back_f=0; + tim.reset(); + } else if(max_lim) back_f=1; + } else { + if(max_lim==1) { + grab++; + back_f=0; + 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()>300) { + grab++; + tim.reset(); + } + break; - //タオルつかむ 四角 - if(grab_f || sgrab_f) { - PALETTE(BLUE); - 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,40); - //sgrab_fは途中のリミットで止まる - if((sgrab_f && short_lim) || max_lim==1) { - grab++; - tim.reset(); - } - break; + case 2: + //爪閉じる + i2c.Out_Set(Out_Data,1,0); + if(tim.read_ms()>100) { + grab++; + tim.reset(); + } + break; + + case 3: + //腕上げる + i2c.Out_Set(Out_Data,0,0); + if(tim.read_ms()>300) { + grab++; + tim.reset(); + } + break; + + //腕戻す + case 4: + MD_SET_DRIVE(MD_Data,4,MD_FORWARD); + MD_SET_PWM (MD_Data,4,40); + if(start_lim==1) { + grab_f=0; + spull_f=0; + tim.reset(); + } + break; + } + } //if(grab_f) - case 1: - //腕停止 - MD_SET_DRIVE(MD_Data,4,MD_BRAKE); - MD_SET_PWM(MD_Data,4,0); + //タオル引く 丸 + else if(pull_f) { + PALETTE(CYAN); + short_f=1; + switch(pull) { + //腕を前に動かす + case 0: + //爪閉じる + i2c.Out_Set(Out_Data,1,0); + + MD_SET_DRIVE(MD_Data,4,MD_REVERSE); + MD_SET_PWM (MD_Data,4,40); + + if(max_lim==1) { + pull++; + back_f=0; + 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()>300) { - grab++; - tim.reset(); - } - break; + //腕おろす + i2c.Out_Set(Out_Data,0,1); + if(tim.read_ms()>300) { + pull++; + tim.reset(); + } + break; + + case 2: + if(tim.read_ms()>300) { + pull++; + tim.reset(); + } + break; - case 2: - //爪閉じる - i2c.Out_Set(Out_Data,1,0); - if(tim.read_ms()>100) { - grab++; - tim.reset(); - } - break; + case 3: + MD_SET_DRIVE(MD_Data,4,MD_FORWARD); + MD_SET_PWM (MD_Data,4,40); + if(start_lim==1) { + pull_f=0; + tim.reset(); + } + break; + } + } //if(pull_f) + + //かごを倒す + else if(kago_f) { + PALETTE(MAGENTA); + short_f=1; + //腕の先を閉じておく + i2c.Out_Set(Out_Data,1,0); - case 3: - //腕上げる - i2c.Out_Set(Out_Data,0,0); - if(tim.read_ms()>300) { - grab++; - tim.reset(); - } - break; + switch(kago) { + //腕を前に動かす + case 0: + MD_SET_DRIVE(MD_Data,4,MD_REVERSE); + MD_SET_PWM (MD_Data,4,40); + 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,80); + if(slide_stop==1) { + kago++; + tim.reset(); + MD_SET_DRIVE(MD_Data,6,MD_BRAKE); + MD_SET_PWM (MD_Data,6,0); + } + 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; - sgrab_f=0; - } - break; - } - } //if(grab_f) + 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,80); + if(slide_start==1) kago++; + break; - //タオル引く 丸 - else if(pull_f) { - PALETTE(CYAN); - switch(pull) { - //腕を前に動かす - case 0: - //爪閉じる - i2c.Out_Set(Out_Data,1,0); + case 4: + MD_SET_DRIVE(MD_Data,4,MD_FORWARD); + MD_SET_PWM (MD_Data,4,40); + if(start_lim==1) { + kago_f=0; + tim.reset(); + } + break; + } + } else { + //爪開く + i2c.Out_Set(Out_Data,1,1); + //腕上げる + i2c.Out_Set(Out_Data,0,0); + + //腕真ん中で停止 + if(short_f) { + if(short_lim==1) short_f=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_PWM (MD_Data,4,20); + } else { 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()>300) { - pull++; - tim.reset(); - } - break; - - case 2: - if(tim.read_ms()>300) { - pull++; - tim.reset(); - } - break; + } + //かご停止 + MD_SET_DRIVE(MD_Data,6,MD_BRAKE); + MD_SET_PWM (MD_Data,6,0); - case 3: - MD_SET_DRIVE(MD_Data,4,MD_FORWARD); - MD_SET_PWM (MD_Data,4,60); - if(start_lim==1) pull_f=0; - break; - } - } //if(pull_f) - - //かごを倒す - else if(kago_f) { - PALETTE(MAGENTA); - //腕の先を閉じておく - i2c.Out_Set(Out_Data,1,0); - - switch(kago) { - //腕を前に動かす - case 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); + kago=0; + pull=0; + grab=0; - //かご回転 - MD_SET_DRIVE(MD_Data,6,MD_FORWARD); - MD_SET_PWM (MD_Data,6,80); - if(slide_stop==1 || tim.read_ms()>3000) { - 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()>1000) { - kago++; - tim.reset(); - } - break; - - case 3: - MD_SET_DRIVE(MD_Data,6,MD_REVERSE); - MD_SET_PWM (MD_Data,6,80); - if(slide_start==1 || tim.read_ms()>3000) 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; - break; + tim.reset(); } - } 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); - - //爪開く - i2c.Out_Set(Out_Data,1,1); - //腕上げる - i2c.Out_Set(Out_Data,0,0); - - kago=0; - pull=0; - grab=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); + } } //-------------- 出力 --------------// //第二引数には使う最大の個数 - i2c.Out(Out_Data,5); + i2c.Out(Out_Data,8); //モータ出力 for(int i=0; i<8; i++) i2c.MD_I2C(MD_Data,i); @@ -343,8 +400,8 @@ /* ----------------ボタン系--------------- */ //角度リセット if(sbdbt.L1) { - yaw=0; - Turn_val=0; + yaw=def_val; + Turn_val=def_val; } //かごを倒す if(CROSS) { @@ -363,8 +420,8 @@ } } else if(TRIANGLE) { if(once==0) { - sgrab_f++; - sgrab_f%=2; + spull_f++; + spull_f%=2; once++; } } @@ -399,7 +456,12 @@ duty[3]=-(lx+ly); for(int i=0; i<4; i++) { + //旋回 duty[i]+=rx; + + //制限 + if(duty[i]>=99) duty[i]=99; + else if(duty[i]<=-99) duty[i]=-99; MD_SET_DRIVE(MD_Data, i, duty[i]==0? MD_BRAKE: (duty[i]>0? MD_FORWARD: MD_REVERSE)); MD_SET_PWM(MD_Data, i, abs(duty[i])); }