Takushima Yukimasa
/
R19_ManuTakukAHWGeo
a
Diff: main.cpp
- Revision:
- 17:446be2c278d1
- Parent:
- 16:829b953d1ac1
diff -r 829b953d1ac1 -r 446be2c278d1 main.cpp --- a/main.cpp Mon Sep 23 08:45:17 2019 +0000 +++ b/main.cpp Tue Sep 24 04:28:34 2019 +0000 @@ -28,7 +28,7 @@ int back_f=0, release_f=0, unfold_f=0, short_f=0; //妨害に使う -int disturb, disturb_f; +int disturb=0, disturb_f=0; //上下動作を保持 //[0]: 上 @@ -58,6 +58,9 @@ // 足回り int lx,ly,rx; +int start_f=0; +int test, test2, test3; + /*----------------------------------- main -----------------------------------*/ int main() { @@ -102,7 +105,7 @@ if(zone==0) PALETTE(RED); else PALETTE(BLUE); - if(release_f) PALETTE(YELLOW); + if(release_f) PALETTE(WHITE); //リミット for(int i=0; i<7; i++) i2c.In(In_Data,i); @@ -118,7 +121,7 @@ if(sbdbt.RX != 64) drift_tim.reset(); //旋回して慣性で動いた後の角度に補正する - if(drift_tim.read_ms()<1000) Turn_val=yaw; + if( drift_tim.read_ms()<1000 ) Turn_val=yaw; //角度補正 AngleCorrection(yaw, Turn_val); @@ -155,22 +158,18 @@ //タオルつかむ 四角 if(grab_f) { - //真ん中で止まる用 - short_f=1; switch(grab) { //腕を前に動かす case 0: - //爪開く - i2c.Out_Set(Out_Data,1,1); - //腕上げる - i2c.Out_Set(Out_Data,0,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(); + start_f=1; } break; @@ -179,54 +178,45 @@ 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) { + 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++; - tim.reset(); } break; + //腕戻す case 2: - //爪閉じる - i2c.Out_Set(Out_Data,1,0); - if(tim.read_ms()>0) { + 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++; - tim.reset(); } break; case 3: - //腕上げる - i2c.Out_Set(Out_Data,0,0); - if(tim.read_ms()>0) { - grab++; - tim.reset(); + if( MyTimer(50, AUT) ) { + MD_SET_DRIVE(MD_Data,4,MD_REVERSE); + MD_SET_PWM (MD_Data,4,30); } - 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(); - } + if(short_lim==1) grab_f=0; 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); + 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); @@ -235,7 +225,7 @@ if(short_lim==1) { spull++; - tim.reset(); + start_f=1; } else if(max_lim) back_f=1; break; @@ -244,115 +234,91 @@ 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) { + i2c.Out_Set(Out_Data,0,1); //腕おろす + if( MyTimer(200, AUT) ) { + i2c.Out_Set(Out_Data,1,0); //爪閉じる spull++; - tim.reset(); } break; + //腕戻す case 2: - //爪閉じる - i2c.Out_Set(Out_Data,1,0); - if(tim.read_ms()>0) { + 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++; - tim.reset(); } break; case 3: - //腕上げる -// i2c.Out_Set(Out_Data,0,0); - if(tim.read_ms()>0) { - spull++; - tim.reset(); - } - break; + MD_SET_DRIVE(MD_Data,4,MD_REVERSE); + MD_SET_PWM (MD_Data,4,30); - //腕戻す - 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(); - } + if(short_lim==1) spull_f=0; 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); + 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++; - tim.reset(); } 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); - //腕おろす - i2c.Out_Set(Out_Data,0,1); - if(tim.read_ms()>100) { - pull++; - tim.reset(); - } + if( MyTimer(100, AUT) ) pull++; break; - //停止 case 2: - if(tim.read_ms()>0) { + 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++; - 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(); - } + 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) { - short_f=1; - switch(kago) { //腕を前に動かす case 0: - //爪閉じる - i2c.Out_Set(Out_Data,1,0); - //腕上げる - i2c.Out_Set(Out_Data,0,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(); - } + + if(max_lim==1) kago++; break; case 1: @@ -363,171 +329,139 @@ //かご回転 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); + kago++; } break; case 2: - if(tim.read_ms()>0) { - kago++; - tim.reset(); - } + 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,6,MD_REVERSE); - MD_SET_PWM (MD_Data,6,100); - if(slide_start==1) kago++; + 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_FORWARD); - MD_SET_PWM (MD_Data,4,60); - if(start_lim==1) { - kago_f=0; - tim.reset(); - } + 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(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); + //妨害 + 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); + 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); + if(max_lim==1) { + start_f=1; + disturb++; + } + break; - //腕おろす - i2c.Out_Set(Out_Data,0,1); - if(tim.read_ms()>200) { - 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); //腕おろす - case 2: - //爪閉じる - i2c.Out_Set(Out_Data,1,0); - if(tim.read_ms()>0) { - disturb++; - tim.reset(); - } - break; + if( MyTimer(200, AUT) ) disturb++; + break; - case 3: - //腕上げる - i2c.Out_Set(Out_Data,0,0); + 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); - } + //赤ゾーン + 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 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(disturb_f) - 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 { + if(sbdbt.R1) { + i2c.Out_Set(Out_Data,0,1); //腕おろす + i2c.Out_Set(Out_Data,1,0); //爪閉じる } else { - //爪開く - i2c.Out_Set(Out_Data,1,1); + i2c.Out_Set(Out_Data,0,0); //腕あげる + 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); - } - } + //奥のリミットに当たったら逆回転 + 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); } - //かご停止 - MD_SET_DRIVE(MD_Data,6,MD_BRAKE); - MD_SET_PWM (MD_Data,6,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; - } + kago=0; + pull=0; + grab=0; + spull=0; + disturb=0; } /* --------------電磁弁系-------------------- */ @@ -565,7 +499,7 @@ else once=0; //半自動のデバック - pc.printf("%d %d ",kago_f, grab_f); + pc.printf("%d %d %d ",disturb, disturb_f, sbdbt.PS); pc.printf("\n\r"); } // while(1) } // int main() @@ -616,8 +550,25 @@ data[1]=data[0]; data[0]=val; - if(data[1]==0 && data[0]==1) { + 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; } \ No newline at end of file