![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
a
main.cpp
- Committer:
- Tom0108
- Date:
- 2019-08-27
- Revision:
- 8:b79d21c8178b
- Parent:
- 6:040d001acb12
- Child:
- 9:64fcbcc182d9
File content as of revision 8:b79d21c8178b:
#include "main.h" /* 型定義 --------------------------------------------------------------------*/ //ゾーン対応 #define _RED_ZONE_ #ifdef _RED_ZONE_ const int def_val=0; #else const int def_val=180; #endif /* 関数宣言 ------------------------------------------------------------------*/ /* 変数宣言 ------------------------------------------------------------------*/ //かご回転用 int kago=0, kago_f=0; //コントローラーの状態を保存するため int once=0; //タオル引く用 int pull=0, pull_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=def_val; //ジャイロさんぷりんぐ用 double sum=0, ave=0; int offset_cnt=0; //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]; // 足回り int lx,ly,rx; int limit_up,limit_down; // リミットスイッチ int slide_start,slide_stop; //かごのリミット int as=0; /*----------------------------------- main -----------------------------------*/ int main() { //flipper1.attach(&flip1, 0.001); // 割り込み while(1) { //自動系のタイマースタート tim.start(); drift_tim.start(); mpu.loop(); //ジャイロから値を取得 y_deg[1] = y_deg[0]; y_deg[0] = (double)mpu.ypr[0] * 180 / M_PI; //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(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; 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; //R1で停止 if(sbdbt.R1==0) { //メカナム基本動作 mecanum_Move(lx, ly, rx); //旋回している間タイマーをリセット if(sbdbt.RX != 64) drift_tim.reset(); //旋回して慣性で動いた後の角度に補正する if(drift_tim.read_ms()<500) 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) { 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); } //タオルつかむ 四角 if(grab_f || spull_f) { PALETTE(GREEN); short_f=1; switch(grab) { //腕を前に動かす case 0: //爪開く 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_PWM (MD_Data,4,40); 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; 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) //タオル引く 丸 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) { pull++; tim.reset(); } break; case 2: if(tim.read_ms()>300) { pull++; 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); 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 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; 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,20); } 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); 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,8); //モータ出力 for(int i=0; i<8; i++) i2c.MD_I2C(MD_Data,i); /* ----------- りみっと --------------- */ limit_up = (In_Data+4)->in_data; limit_down = (In_Data+5)->in_data; max_lim = (In_Data+1)->in_data; start_lim =(In_Data+2)->in_data; short_lim = (In_Data+6)->in_data; 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("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("%2d ",dir); 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(sbdbt.L1) { yaw=def_val; Turn_val=def_val; } //かごを倒す if(CROSS) { if(once==0) { kago_f++; kago_f%=2; once++; } } //タオルを掴んで動かす else if(SQUARE) { if(once==0) { grab_f++; grab_f%=2; once++; } } else if(TRIANGLE) { if(once==0) { spull_f++; spull_f%=2; once++; } } //倒すを引いて動かす else if(CIRCLE) { if(once==0) { pull_f++; pull_f%=2; once++; } } //タオル展開用 else if(sbdbt.START) { if(once==0) { unfold_f++; unfold_f%=2; once++; } } else once=0; } // if(sbdbt.State_check()) } // while(1) } // int main() /* メカナムの基本移動 */ void mecanum_Move(int lx, int ly, int rx) { duty[0]=lx+ly; duty[1]=-(-lx+ly); duty[2]=-lx+ly; 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])); } } void AngleCorrection(double n_angle, double t_angle) { double dif=-(t_angle-n_angle); if(dif>=30) dif=30; else if(dif<=-30) dif=-30; for(int i=0; i<4; i++) { duty[i]+=dif; 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])); } } /*操作法*/ /* 〇 ボタン == 回収(引きずる) ? ボタン == 籠 △ ボタン == 回収(シーツ兼シャツ) □ ボタン == 回収(タオル) 上 ボタン == ハンガーかけ機構 上昇 下 ボタン == ハンガーかけ機構 下降 L1 ボタン == リセット(足回り) L2 ボタン == タオル掛け R2 ボタン == タオル真ん中掴む select == ハンガー掛け 最大まで上昇 start == ハンガー展開 */ /*その他注意点*/ /* 動かすときはジャイロの値を安定させるためLEDが赤から緑になるまで待つ メカナムは振動が大きいせいかナット等が外れやすいため、定期的に確認をする(特に足回り) あまり急発進をさせない(たまに暴走) 暴走したときはコントローラーを動かしたら治るはず コントローラー接続が切れたらSTMの黒い奴を押す(わからないときは近くの回路班へ) */