Takushima Yukimasa
/
R19_ManuTakukAHWGeo
a
main.cpp
- Committer:
- Tom0108
- Date:
- 2019-09-24
- Revision:
- 17:446be2c278d1
- Parent:
- 16:829b953d1ac1
File content as of revision 17:446be2c278d1:
#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=0, disturb_f=0; //上下動作を保持 //[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 start_f=0; int test, test2, test3; /*----------------------------------- 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; //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); if(release_f) PALETTE(WHITE); //リミット 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); 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; //メカナム基本動作 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) { 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++; start_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( MyTimer(200, AUT) ) { i2c.Out_Set(Out_Data,1,0); //爪閉じる i2c.Out_Set(Out_Data,0,0); //腕上げる grab++; } break; //腕戻す case 2: MD_SET_DRIVE(MD_Data,4,MD_FORWARD); MD_SET_PWM (MD_Data,4,60); if(start_lim==1) { i2c.Out_Set(Out_Data,1,1); //爪開く start_f=1; grab++; } break; case 3: if( MyTimer(50, AUT) ) { MD_SET_DRIVE(MD_Data,4,MD_REVERSE); MD_SET_PWM (MD_Data,4,30); } if(short_lim==1) grab_f=0; break; } } //if(grab_f) //シーツ用 else if(spull_f) { 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++; start_f=1; } 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( MyTimer(200, AUT) ) { i2c.Out_Set(Out_Data,1,0); //爪閉じる spull++; } break; //腕戻す case 2: MD_SET_DRIVE(MD_Data,4,MD_FORWARD); MD_SET_PWM (MD_Data,4,60); if(start_lim==1) { i2c.Out_Set(Out_Data,0,0); //腕上げる spull++; } break; case 3: MD_SET_DRIVE(MD_Data,4,MD_REVERSE); MD_SET_PWM (MD_Data,4,30); if(short_lim==1) spull_f=0; break; } } //if(spull_f) //タオル引く 丸 else if(pull_f) { 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) { start_f=1; pull++; } break; case 1: i2c.Out_Set(Out_Data,0,1); //腕おろす //腕停止 MD_SET_DRIVE(MD_Data,4,MD_BRAKE); MD_SET_PWM (MD_Data,4,0); if( MyTimer(100, AUT) ) pull++; break; case 2: MD_SET_DRIVE(MD_Data,4,MD_FORWARD); MD_SET_PWM (MD_Data,4,60); if(start_lim==1) { i2c.Out_Set(Out_Data,0,0); //腕上げる pull++; } break; case 3: MD_SET_DRIVE(MD_Data,4,MD_REVERSE); MD_SET_PWM (MD_Data,4,30); if(short_lim==1) pull_f=0; break; } } //if(pull_f) //かごを倒す else if(kago_f) { 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++; 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) { MD_SET_DRIVE(MD_Data,6,MD_BRAKE); MD_SET_PWM (MD_Data,6,0); kago++; } break; case 2: MD_SET_DRIVE(MD_Data,6,MD_REVERSE); MD_SET_PWM (MD_Data,6,100); if(slide_start==1) kago++; break; case 3: MD_SET_DRIVE(MD_Data,4,MD_FORWARD); MD_SET_PWM (MD_Data,4,60); if(start_lim==1) kago++; break; case 4: MD_SET_DRIVE(MD_Data,4,MD_REVERSE); MD_SET_PWM (MD_Data,4,30); if(short_lim==1) kago_f=0; break; } } //妨害 else if(disturb_f) { 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) { start_f=1; disturb++; } 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( MyTimer(200, AUT) ) disturb++; break; case 2: i2c.Out_Set(Out_Data,1,0); //爪閉じる 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(sbdbt.R1) { i2c.Out_Set(Out_Data,0,1); //腕おろす i2c.Out_Set(Out_Data,1,0); //爪閉じる } else { i2c.Out_Set(Out_Data,0,0); //腕あげる i2c.Out_Set(Out_Data,1,1); //爪開く } //奥のリミットに当たったら逆回転 if(max_lim==1) back_f=1; //手前のリミットに当たったら正回転 else if(start_lim==1) back_f=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); } } //止まる 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; spull=0; disturb=0; } /* --------------電磁弁系-------------------- */ i2c.Out_Set(Out_Data, 2, sbdbt.L2); //タオル掛け i2c.Out_Set(Out_Data, 4, sbdbt.R2); //タオルを引っ張るやつ i2c.Out_Set(Out_Data, 3, unfold_f); //タオル展開 i2c.Out_Set(Out_Data, 5, release_f);//エアー解放 //-------------- 出力 --------------/ 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; /* ----------------ボタン系--------------- */ //角度リセット if(sbdbt.L1) { yaw=def_val; Turn_val=def_val; } if(CROSS) Toggle(&kago_f, CROSS); //かごを倒す else if(SQUARE) Toggle(&grab_f, SQUARE); //タオルを掴んで動かす else if(TRIANGLE) Toggle(&spull_f, TRIANGLE); else if(CIRCLE) Toggle(&pull_f, CIRCLE); else if(sbdbt.START) Toggle(&unfold_f, sbdbt.START); else if(sbdbt.SELECT) Toggle(&release_f, sbdbt.SELECT); else if(sbdbt.PS) Toggle(&disturb_f, sbdbt.PS); else once=0; //半自動のデバック pc.printf("%d %d %d ",disturb, disturb_f, sbdbt.PS); 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])); } } void Toggle(int *variable, int val) { static int data[2]= {0}; if(once==0) { data[0]=0; once=1; } data[1]=data[0]; data[0]=val; if(data[1]==0 && data[0]!=0) { (*variable)++; (*variable)%=2; } } int MyTimer(int t, int num) { static int tmp[TIM_MAX]; static int msec[TIM_MAX]; if(start_f) { tmp[num]=tim.read_ms(); msec[num]=t; start_f=0; } pc.printf("%d %d %d ",tmp[num], tim.read_ms(), msec[num]); if( tim.read_ms()-tmp[num]>=msec[num] ) return 1; else return 0; }