Takushima Yukimasa
/
R19_ManuTakukAHWGeo
a
Diff: main.cpp
- Revision:
- 10:7c73e1577742
- Parent:
- 9:64fcbcc182d9
- Child:
- 11:c1a22e2e3534
diff -r 64fcbcc182d9 -r 7c73e1577742 main.cpp --- a/main.cpp Tue Aug 27 10:59:38 2019 +0000 +++ b/main.cpp Fri Aug 30 07:47:41 2019 +0000 @@ -18,25 +18,24 @@ //コントローラーの状態を保存するため int once=0; -//タオル引く用 -int pull=0, pull_f=0; - //タオル掴む用 -//spull_f: 中央のリミットでつかむ +//pull: 奥のリミットで引く +//spull: 中央のリミットでつかむ +//grab: 掴む //back_f: 真ん中目で下がるとき -int grab=0, grab_f=0, spull_f=0; +int pull=0, pull_f=0, grab=0, grab_f=0, spull=0, spull_f=0; int back_f=0; //タオル横展開用 int unfold_f=0; -//y_deg: ジャイロの生の値 //yaw: 実際に使う角度 -double y_deg[2], yaw=def_val; +double yaw=def_val; -//ジャイロさんぷりんぐ用 -double sum=0, ave=0; -int offset_cnt=0; +//[0]: roll +//[1]: pitch +//[2]: yaw +double rawyaw[2]; //Turn_val: 補正の目標値 //dif_val: 補正値との差 @@ -62,43 +61,36 @@ /*----------------------------------- main -----------------------------------*/ int main() { - //flipper1.attach(&flip1, 0.001); // 割り込み + bno.reset(); while(1) { + //ゾーン対応 +#ifdef _RED_ZONE_ + PALETTE(RED); +#else + PALETTE(BLUE); +#endif + + bno.setmode(OPERATION_MODE_IMUPLUS); + //自動系のタイマースタート tim.start(); drift_tim.start(); - mpu.loop(); - //ジャイロから値を取得 - y_deg[1] = y_deg[0]; - y_deg[0] = (double)mpu.ypr[0] * 180 / M_PI; - + //角度の取得 + bno.get_angles(); + rawyaw[1]=rawyaw[0]; + rawyaw[0]=bno.euler.yaw; + //180~-180をまたいだ時 - if(y_deg[1]<-90 && y_deg[0]>90) y_deg[1]+=360; - else if(y_deg[1]>90 && y_deg[0]<-90) y_deg[1]-=360; + 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); - //誤差をさんぷりんぐ - if(offset_cnt<1000) { - PALETTE(YELLOW); - if(offset_cnt!=0) { - sum+=(y_deg[0]-y_deg[1]); - ave=sum/offset_cnt; - } - offset_cnt++; - } else { - //ゾーン対応 -#ifdef _RED_ZONE_ - PALETTE(RED); -#else - PALETTE(BLUE); -#endif - yaw -= (y_deg[0]-y_deg[1])-ave; - } - dif_val = yaw-Turn_val; //制限 if(dif_val>=30) dif_val=30; @@ -145,10 +137,10 @@ } //タオルつかむ 四角 - if(grab_f || spull_f) { + if(grab_f) { PALETTE(GREEN); - - //真ん中で止まる用s + + //真ん中で止まる用 short_f=1; switch(grab) { //腕を前に動かす @@ -156,22 +148,12 @@ //爪開く i2c.Out_Set(Out_Data,1,1); - if(back_f) MD_SET_DRIVE(MD_Data,4,MD_FORWARD); - else MD_SET_DRIVE(MD_Data,4,MD_REVERSE); + MD_SET_DRIVE(MD_Data,4,MD_REVERSE); MD_SET_PWM (MD_Data,4,60); - 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(); - } + if(max_lim==1) { + grab++; + tim.reset(); } break; @@ -212,13 +194,74 @@ MD_SET_PWM (MD_Data,4,60); if(start_lim==1) { grab_f=0; - spull_f=0; tim.reset(); } break; } } //if(grab_f) + //シーツ用 + else if(spull_f) { + PALETTE(WHITE); + short_f=1; + switch(spull) { + //腕を前に動かす + case 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,60); + + 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; + + 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; + + } + } //if(spull_f) + //タオル引く 丸 else if(pull_f) { PALETTE(CYAN); @@ -234,7 +277,6 @@ if(max_lim==1) { pull++; - back_f=0; tim.reset(); } break; @@ -343,7 +385,7 @@ 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); @@ -358,7 +400,7 @@ kago=0; pull=0; grab=0; - + spull=0; tim.reset(); } } @@ -390,12 +432,11 @@ // 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("yaw:%.1f dif:%.1f",yaw,dif_val); - pc.printf("0:%.1f 1:%.1f yaw:%.1f dif:%.1f",y_deg[0],y_deg[1],yaw,dif_val); + pc.printf("%.1f %.1f ",rawyaw[0],rawyaw[1]); + pc.printf("yaw:%.1f dif:%.1f",yaw,dif_val); // pc.printf("%2d ",dir); pc.printf("\n\r"); - /* --------------電磁弁系-------------------- */ /*タオル掛け*/ if(sbdbt.L2)i2c.Out_Set(Out_Data,2,1); @@ -408,7 +449,6 @@ if(unfold_f)i2c.Out_Set(Out_Data,3,1); else i2c.Out_Set(Out_Data,3,0); - /* ----------------ボタン系--------------- */ //角度リセット if(sbdbt.L1) { @@ -491,7 +531,6 @@ 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])); } - } /*操作法*/ @@ -507,12 +546,4 @@ R2 ボタン == タオル真ん中掴む select == ハンガー掛け 最大まで上昇 start == ハンガー展開 -*/ -/*その他注意点*/ -/* -動かすときはジャイロの値を安定させるためLEDが赤から緑になるまで待つ -メカナムは振動が大きいせいかナット等が外れやすいため、定期的に確認をする(特に足回り) -あまり急発進をさせない(たまに暴走) -暴走したときはコントローラーを動かしたら治るはず -コントローラー接続が切れたらSTMの黒い奴を押す(わからないときは近くの回路班へ) */ \ No newline at end of file