Takushima Yukimasa
/
R19_ManuTakukAHWGeo
a
Diff: main.cpp
- Revision:
- 11:c1a22e2e3534
- Parent:
- 10:7c73e1577742
- Child:
- 12:7e33e400a48d
diff -r 7c73e1577742 -r c1a22e2e3534 main.cpp --- a/main.cpp Fri Aug 30 07:47:41 2019 +0000 +++ b/main.cpp Wed Sep 04 05:31:40 2019 +0000 @@ -38,8 +38,7 @@ double rawyaw[2]; //Turn_val: 補正の目標値 -//dif_val: 補正値との差 -double Turn_val=def_val, dif_val; +double Turn_val=def_val; //short_lim: 腕中央 //max_lim: 腕奥側 @@ -58,9 +57,12 @@ int as=0; +int release_f=0; + /*----------------------------------- main -----------------------------------*/ int main() { + //ジャイロリセット bno.reset(); while(1) { @@ -71,31 +73,25 @@ PALETTE(BLUE); #endif - bno.setmode(OPERATION_MODE_IMUPLUS); - //自動系のタイマースタート tim.start(); drift_tim.start(); + bno.setmode(OPERATION_MODE_IMUPLUS); //角度の取得 bno.get_angles(); rawyaw[1]=rawyaw[0]; rawyaw[0]=bno.euler.yaw; - + //180~-180をまたいだ時 if(rawyaw[1]<90 && rawyaw[0]>270) rawyaw[1]+=360; else if(rawyaw[1]>270 && rawyaw[0]<90) rawyaw[1]-=360; - + yaw-=rawyaw[0]-rawyaw[1]; if(sbdbt.State_check()) { for(int i=0; i<7; i++) i2c.In(In_Data,i); - dif_val = yaw-Turn_val; - //制限 - if(dif_val>=30) dif_val=30; - else if(dif_val<=-30) dif_val=-30; - 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; @@ -109,25 +105,27 @@ if(sbdbt.RX != 64) drift_tim.reset(); //旋回して慣性で動いた後の角度に補正する - if(drift_tim.read_ms()<500) Turn_val=yaw; + if(drift_tim.read_ms()<1000) Turn_val=yaw; //角度補正 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(UPkey && limit_up == 0) { +// 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; 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); @@ -138,8 +136,6 @@ //タオルつかむ 四角 if(grab_f) { - PALETTE(GREEN); - //真ん中で止まる用 short_f=1; switch(grab) { @@ -202,7 +198,6 @@ //シーツ用 else if(spull_f) { - PALETTE(WHITE); short_f=1; switch(spull) { //腕を前に動かす @@ -210,15 +205,15 @@ //真ん中のリミットより前に腕があった場合にバックする 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,60); + MD_SET_PWM (MD_Data,4,30); if(short_lim==1) { spull++; tim.reset(); } else if(max_lim) back_f=1; break; - - case 1: + + case 1: //腕停止 MD_SET_DRIVE(MD_Data,4,MD_BRAKE); MD_SET_PWM(MD_Data,4,0); @@ -264,7 +259,6 @@ //タオル引く 丸 else if(pull_f) { - PALETTE(CYAN); short_f=1; switch(pull) { //腕を前に動かす @@ -315,7 +309,7 @@ //かごを倒す else if(kago_f) { - PALETTE(MAGENTA); +// PALETTE(MAGENTA); short_f=1; //腕の先を閉じておく i2c.Out_Set(Out_Data,1,0); @@ -432,9 +426,9 @@ // 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 dif:%.1f",yaw,dif_val); -// pc.printf("%2d ",dir); +// 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("\n\r"); /* --------------電磁弁系-------------------- */ @@ -446,9 +440,16 @@ 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); + //タオル展開 + 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) { @@ -492,6 +493,14 @@ unfold_f%=2; once++; } + } + //タオル展開用 + else if(sbdbt.SELECT) { + if(once==0) { + release_f++; + release_f%=2; + once++; + } } else once=0;