Takushima Yukimasa
/
R19_ManuTakukAHWGeo
a
main.cpp
- Committer:
- Tom0108
- Date:
- 2019-10-11
- Revision:
- 20:3f68738c4019
- Parent:
- 19:340fc4370258
File content as of revision 20:3f68738c4019:
#include "main.h" /* 型定義 --------------------------------------------------------------------*/ /* 関数宣言 ------------------------------------------------------------------*/ /* 変数宣言 ------------------------------------------------------------------*/ //0: 赤 //1: 青 int zone=0; //デフォルトの角度 int def_val=0; //コントローラーの状態を保存するため int once=0; //kogo: かごをひっくり返す //pull: 奥のリミットで引く //spull: 中央のリミットでつかむ(short_pull //grab: 掴む //back_f: 真ん中目で下がるとき //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 disturb, disturb_f; //上下動作を保持 //[0]: 上 //[1]: 下 int keep_f[2]= {0}; //実際に使う角度 double yaw; //[0]: 今回の値 //[1]: 前回の値 double rawyaw[2]; //Turn_val: 補正の目標値 double Turn_val; //short_lim: 腕中央 //max_lim: 腕奥側 //short_lim: 腕手前側 int short_lim, max_lim, start_lim; int limit_up,limit_down; // リミットスイッチ int slide_start,slide_stop; //かごのリミット //足回りのpwm値 int duty[4]; // 足回り int lx,ly,rx; int rod_hit=0; /*----------------------------------- main -----------------------------------*/ int main() { //リミット i2c.In(In_Data,7); zone=(In_Data+7)->in_data; if(zone==0) def_val=0; else def_val=180; yaw=def_val; //補正角度 Turn_val=def_val; sbdbt.LX=64; sbdbt.LY=64; sbdbt.RX=64; sbdbt.RY=64; //ジャイロリセット bno.reset(); while(1) { //自動系のタイマースタート tim.start(); drift_tim.start(); keep_tim[0].start(); keep_tim[1].start(); bno.setmode(OPERATION_MODE_IMUPLUS); //角度の取得 bno.get_angles(); rawyaw[1]=rawyaw[0]; rawyaw[0]=bno.euler.yaw; if(rawyaw[0]!=0) { //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]; yaw=fmod(yaw, 360.0); } //ゾーン対応 if(zone==0) PALETTE(RED); else PALETTE(BLUE); //リミット for(int i=0; i<9; 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); 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(1) { //メカナム基本動作 mecanum_Move(lx, ly, rx); //旋回している間タイマーをリセット if(sbdbt.RX != 64) drift_tim.reset(); //旋回して慣性で動いた後の角度に補正する if(drift_tim.read_ms()<1000) Turn_val=yaw; //角度補正 AngleCorrection(yaw, Turn_val); /*---ハンガーかけるやつ----------------------------------------------*/ //上方向 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 { 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); } //タオルつかむ 四角 if(grab_f) { //真ん中で止まる用 short_f=1; switch(grab) { //腕を前に動かす case 0: //爪開く i2c.Out_Set(Out_Data,1,1); //腕上げる i2c.Out_Set(Out_Data,0,0); MD_SET_DRIVE(MD_Data,4,MD_REVERSE); MD_SET_PWM (MD_Data,4,60); if(max_lim==1) { grab++; 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()>200) { grab++; tim.reset(); } break; case 2: //爪閉じる i2c.Out_Set(Out_Data,1,0); if(tim.read_ms()>0) { grab++; tim.reset(); } break; case 3: //腕上げる i2c.Out_Set(Out_Data,0,0); if(tim.read_ms()>0) { grab++; tim.reset(); } 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; tim.reset(); } break; } } //if(grab_f) //シーツ用 else if(spull_f) { short_f=1; switch(spull) { //腕を前に動かす case 0: //爪閉じる i2c.Out_Set(Out_Data,1,0); //腕上げる i2c.Out_Set(Out_Data,0,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); 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) { short_f=1; switch(pull) { //腕を前に動かす case 0: //爪閉じる i2c.Out_Set(Out_Data,1,0); //腕上げる i2c.Out_Set(Out_Data,0,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_DRIVE(MD_Data,4,MD_BRAKE); MD_SET_PWM (MD_Data,4,0); //腕おろす i2c.Out_Set(Out_Data,0,1); if(tim.read_ms()>100) { pull++; tim.reset(); } break; //停止 case 2: if(tim.read_ms()>0) { pull++; tim.reset(); } break; case 3: MD_SET_DRIVE(MD_Data,4,MD_FORWARD); MD_SET_PWM (MD_Data,4,60); if(start_lim==1) { pull_f=0; tim.reset(); } break; } } //if(pull_f) //かごを倒す else if(kago_f) { // PALETTE(MAGENTA); short_f=1; switch(kago) { //腕を前に動かす case 0: //爪閉じる i2c.Out_Set(Out_Data,1,0); //腕上げる i2c.Out_Set(Out_Data,0,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); //かご回転 MD_SET_DRIVE(MD_Data,6,MD_FORWARD); MD_SET_PWM (MD_Data,6,100); 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,100); if(slide_start==1) 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; tim.reset(); } break; } } else { kago=0; pull=0; grab=0; spull=0; if(sbdbt.R1) { //腕おろす i2c.Out_Set(Out_Data,0,1); //爪閉じる i2c.Out_Set(Out_Data,1,0); } //腕あげる else i2c.Out_Set(Out_Data,0,0); if(disturb_f) { //真ん中で止まる用 short_f=1; switch(disturb) { //腕を前に動かす case 0: //爪開く i2c.Out_Set(Out_Data,1,1); //腕上げる i2c.Out_Set(Out_Data,0,0); MD_SET_DRIVE(MD_Data,4,MD_REVERSE); MD_SET_PWM (MD_Data,4,60); if(max_lim==1) { disturb++; 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()>200) { disturb++; tim.reset(); } break; case 2: //爪閉じる i2c.Out_Set(Out_Data,1,0); if(tim.read_ms()>0) { disturb++; tim.reset(); } break; case 3: //腕上げる i2c.Out_Set(Out_Data,0,0); //赤ゾーン if( yaw<=90 || 270<yaw ) { MD_SET_PWM(MD_Data,4,20); if(RIGHTkey && max_lim==0) MD_SET_DRIVE(MD_Data,4,MD_REVERSE); else if(LEFTkey && start_lim==0) MD_SET_DRIVE(MD_Data,4,MD_FORWARD); else { MD_SET_DRIVE(MD_Data,4,MD_BRAKE); MD_SET_PWM (MD_Data,4,0); } } //青ゾーン else if( 90<yaw && yaw<=270 ) { MD_SET_PWM(MD_Data,4,20); if(LEFTkey && max_lim==0) MD_SET_DRIVE(MD_Data,4,MD_REVERSE); else if(RIGHTkey && start_lim==0) MD_SET_DRIVE(MD_Data,4,MD_FORWARD); else { MD_SET_DRIVE(MD_Data,4,MD_BRAKE); MD_SET_PWM (MD_Data,4,0); } } } } //if(disturb_f) //腕真ん中で停止 else if(short_f) { //爪開く i2c.Out_Set(Out_Data,1,1); if(short_lim==1) { short_f=0; back_f=0; } //奥のリミットに当たったら逆回転 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); } else { //爪開く i2c.Out_Set(Out_Data,1,1); //赤ゾーン if( yaw<=90 || 270<yaw ) { MD_SET_PWM(MD_Data,4,20); if(RIGHTkey && max_lim==0) MD_SET_DRIVE(MD_Data,4,MD_REVERSE); else if(LEFTkey && start_lim==0) MD_SET_DRIVE(MD_Data,4,MD_FORWARD); else { MD_SET_DRIVE(MD_Data,4,MD_BRAKE); MD_SET_PWM (MD_Data,4,0); } } //青ゾーン else if( 90<yaw && yaw<=270 ) { MD_SET_PWM(MD_Data,4,20); if(LEFTkey && max_lim==0) MD_SET_DRIVE(MD_Data,4,MD_REVERSE); else if(RIGHTkey && start_lim==0) MD_SET_DRIVE(MD_Data,4,MD_FORWARD); else { MD_SET_DRIVE(MD_Data,4,MD_BRAKE); MD_SET_PWM (MD_Data,4,0); } } //止まる 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); } } } //非常停止 else { for(int i=0; i<8; i++) { MD_SET_DRIVE(MD_Data,i,MD_BRAKE); MD_SET_PWM (MD_Data,i,0); } } /* --------------電磁弁系-------------------- */ /*タオルを引っ張るやつ*/ if(sbdbt.R2)i2c.Out_Set(Out_Data,3,1); else i2c.Out_Set(Out_Data,3,0); //タオル展開 if(unfold_f) i2c.Out_Set(Out_Data,5,1); else i2c.Out_Set(Out_Data,5,0); //エアー解放 if(release_f) { PALETTE(YELLOW); i2c.Out_Set(Out_Data,4,1); } else i2c.Out_Set(Out_Data,4,0); if(!rod_hit) PALETTE(CYAN); /*タオル掛け*/ if(sbdbt.L2) i2c.Out_Set(Out_Data,2,1); else i2c.Out_Set(Out_Data,2,0); //-------------- 出力 --------------// //第二引数には使う最大の個数 i2c.Out(Out_Data,9); //モータ出力 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; rod_hit = (In_Data+8)->in_data; /* ----------------ボタン系--------------- */ //角度リセット 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 if(sbdbt.SELECT) { if(once==0) { release_f++; release_f%=2; once++; } } else if(sbdbt.PS) { if(once==0) { disturb=0; disturb_f++; disturb_f%=2; once++; } } else once=0; //半自動のデバック // 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 %.1f",yaw, bno.euler.yaw); // pc.printf("%d %d",keep_tim[0].read_ms(),keep_f[0]); // pc.printf("LX:%d, LY:%d, RX:%d, RY:%d",sbdbt.LX, sbdbt.LY, sbdbt.RX, sbdbt.RY); // for(int i=0; i<9; i++) // pc.printf("%d ",(In_Data+i)->in_data); // pc.printf("\n\r"); } // 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 == ハンガー展開 */