Takushima Yukimasa
/
R19_ManuTakukAHWGeo
a
Diff: main.cpp
- Revision:
- 12:7e33e400a48d
- Parent:
- 11:c1a22e2e3534
- Child:
- 13:48c4a4c95e77
diff -r c1a22e2e3534 -r 7e33e400a48d main.cpp --- a/main.cpp Wed Sep 04 05:31:40 2019 +0000 +++ b/main.cpp Sun Sep 08 09:57:22 2019 +0000 @@ -2,7 +2,7 @@ /* 型定義 --------------------------------------------------------------------*/ //ゾーン対応 -#define _RED_ZONE_ +//#define _RED_ZONE_ #ifdef _RED_ZONE_ const int def_val=0; #else @@ -12,29 +12,30 @@ /* 関数宣言 ------------------------------------------------------------------*/ /* 変数宣言 ------------------------------------------------------------------*/ -//かご回転用 -int kago=0, kago_f=0; //コントローラーの状態を保存するため int once=0; -//タオル掴む用 +//kogo: かごをひっくり返す //pull: 奥のリミットで引く -//spull: 中央のリミットでつかむ +//spull: 中央のリミットでつかむ(short_pull) //grab: 掴む //back_f: 真ん中目で下がるとき -int pull=0, pull_f=0, grab=0, grab_f=0, spull=0, spull_f=0; -int back_f=0; +//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; -//タオル横展開用 -int unfold_f=0; +//上下動作を保持 +//[0]: 上 +//[1]: 下 +int keep_f[2]= {0}; //yaw: 実際に使う角度 double yaw=def_val; -//[0]: roll -//[1]: pitch -//[2]: yaw +//[0]: 今回の値 +//[1]: 前回の値 double rawyaw[2]; //Turn_val: 補正の目標値 @@ -44,7 +45,8 @@ //max_lim: 腕奥側 //short_lim: 腕手前側 int short_lim, max_lim, start_lim; -int short_f=0; +int limit_up,limit_down; // リミットスイッチ +int slide_start,slide_stop; //かごのリミット //足回りのpwm値 int duty[4]; @@ -52,13 +54,6 @@ // 足回り int lx,ly,rx; -int limit_up,limit_down; // リミットスイッチ -int slide_start,slide_stop; //かごのリミット - -int as=0; - -int release_f=0; - /*----------------------------------- main -----------------------------------*/ int main() { @@ -66,16 +61,11 @@ bno.reset(); while(1) { - //ゾーン対応 -#ifdef _RED_ZONE_ - PALETTE(RED); -#else - PALETTE(BLUE); -#endif - //自動系のタイマースタート tim.start(); drift_tim.start(); + keep_tim[0].start(); + keep_tim[1].start(); bno.setmode(OPERATION_MODE_IMUPLUS); //角度の取得 @@ -89,7 +79,15 @@ yaw-=rawyaw[0]-rawyaw[1]; + //ゾーン対応 +#ifdef _RED_ZONE_ + PALETTE(RED); +#else + PALETTE(BLUE); +#endif + if(sbdbt.State_check()) { + //リミット 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); @@ -111,25 +109,31 @@ AngleCorrection(yaw, Turn_val); /*---ハンガーかけるやつ----------------------------------------------*/ -// if(sbdbt.SELECT)as=1;//selectキー -// -// if(as==1 && limit_up == 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); -// } - if(UPkey && limit_up == 0) { - as=0; + //上方向 + 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(); + MD_SET_DRIVE(MD_Data, 5,MD_REVERSE); MD_SET_PWM(MD_Data, 5,100); - } else if(DOWNkey && limit_down == 0) { - MD_SET_DRIVE(MD_Data, 5,MD_FORWARD); - 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); } @@ -406,9 +410,28 @@ } } + /* --------------電磁弁系-------------------- */ + /*タオル掛け*/ + 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,8); + i2c.Out(Out_Data,9); //モータ出力 for(int i=0; i<8; i++) i2c.MD_I2C(MD_Data,i); @@ -428,28 +451,10 @@ // 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("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.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(MAGENTA); - i2c.Out_Set(Out_Data,5,1); - } else i2c.Out_Set(Out_Data,5,0); - /* ----------------ボタン系--------------- */ //角度リセット if(sbdbt.L1) { @@ -505,6 +510,7 @@ } // if(sbdbt.State_check()) + else PALETTE(BLACK); } // while(1) } // int main()