Takushima Yukimasa
a
Diff: main.cpp
- Revision:
- 17:446be2c278d1
- Parent:
- 16:829b953d1ac1
--- 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