Takushima Yukimasa
a
Diff: main.cpp
- Revision:
- 16:829b953d1ac1
- Parent:
- 15:68720ff6bbc9
- Child:
- 17:446be2c278d1
--- a/main.cpp Mon Sep 23 06:10:11 2019 +0000
+++ b/main.cpp Mon Sep 23 08:45:17 2019 +0000
@@ -61,17 +61,15 @@
/*----------------------------------- 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;
+ yaw=def_val; //初期角度
+ Turn_val=def_val; //補正角度
sbdbt.LX=64;
sbdbt.LY=64;
@@ -104,6 +102,8 @@
if(zone==0) PALETTE(RED);
else PALETTE(BLUE);
+ if(release_f) PALETTE(YELLOW);
+
//リミット
for(int i=0; i<7; i++) i2c.In(In_Data,i);
@@ -111,55 +111,302 @@
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);
+ //メカナム基本動作
+ 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();
- //旋回している間タイマーをリセット
- if(sbdbt.RX != 64) drift_tim.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(drift_tim.read_ms()<1000) Turn_val=yaw;
+ //タオルつかむ 四角
+ 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);
- //角度補正
- AngleCorrection(yaw, Turn_val);
+ //腕おろす
+ 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;
- /*---ハンガーかけるやつ----------------------------------------------*/
- //上方向
- 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();
+ //腕戻す
+ 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;
- MD_SET_DRIVE(MD_Data, 5,MD_FORWARD);
- MD_SET_PWM(MD_Data, 5,100);
+ 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;
+
}
- //下方向
- 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();
+ } //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;
- 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();
+ 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) {
+ 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_PWM(MD_Data,5,0);
- MD_SET_DRIVE(MD_Data, 5,MD_BRAKE);
+ //かご回転
+ 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 {
+ 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(grab_f) {
+ if(disturb_f) {
//真ん中で止まる用
short_f=1;
- switch(grab) {
+ switch(disturb) {
//腕を前に動かす
case 0:
//爪開く
@@ -171,7 +418,7 @@
MD_SET_PWM (MD_Data,4,60);
if(max_lim==1) {
- grab++;
+ disturb++;
tim.reset();
}
break;
@@ -184,72 +431,7 @@
//腕おろす
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++;
+ disturb++;
tim.reset();
}
break;
@@ -258,314 +440,105 @@
//爪閉じる
i2c.Out_Set(Out_Data,1,0);
if(tim.read_ms()>0) {
- spull++;
+ disturb++;
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();
+ //赤ゾーン
+ 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);
+ }
}
- 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();
+ //青ゾーン
+ 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);
+ }
}
- 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);
-
- 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(disturb_f)
- //腕おろす
- 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);
- //腕真ん中で停止
- 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(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( 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(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);
}
- //かご停止
- 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);
+ }
+ }
- kago=0;
- pull=0;
- grab=0;
- spull=0;
+ //止まる
+ else {
+ MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
+ MD_SET_PWM (MD_Data,4,0);
}
- }
- }
- //非常停止
- else {
- for(int i=0; i<8; i++) {
- MD_SET_DRIVE(MD_Data,i,MD_BRAKE);
- MD_SET_PWM (MD_Data,i,0);
+ //かご停止
+ MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
+ MD_SET_PWM (MD_Data,6,0);
+
+ kago=0;
+ pull=0;
+ grab=0;
+ spull=0;
}
}
/* --------------電磁弁系-------------------- */
- /*タオル掛け*/
- 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);
+ 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);//エアー解放
- //エアー解放
- if(release_f) {
- PALETTE(YELLOW);
- i2c.Out_Set(Out_Data,5,1);
- } else i2c.Out_Set(Out_Data,5,0);
-
- //-------------- 出力 --------------//
- //第二引数には使う最大の個数
- i2c.Out(Out_Data,9);
-
- //モータ出力
- for(int i=0; i<8; i++) i2c.MD_I2C(MD_Data,i);
+ //-------------- 出力 --------------/
+ 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;
@@ -582,71 +555,17 @@
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;
+ 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("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<7; i++)
- pc.printf("%d ",sbdbt.PS);
- pc.printf("%d ",disturb);
+ pc.printf("%d %d ",kago_f, grab_f);
pc.printf("\n\r");
} // while(1)
} // int main()
@@ -687,7 +606,6 @@
void Toggle(int *variable, int val)
{
- static int once=0;
static int data[2]= {0};
if(once==0) {
@@ -695,39 +613,11 @@
once=1;
}
- //値の更新
data[1]=data[0];
data[0]=val;
if(data[1]==0 && data[0]==1) {
- *variable++;
- *variable%=2;
- } else if(data[1]==1 && data[0]==0) once=0;
-}
-
-/*操作法*/
-/*
-〇 ボタン == 回収（引きずる）
-? ボタン == 籠
-△ ボタン == 回収（シーツ兼シャツ）
-□ ボタン == 回収（タオル）
-上 ボタン == ハンガーかけ機構 上昇
-下 ボタン == ハンガーかけ機構 下降
-L1 ボタン == リセット（足回り）
-L2 ボタン == タオル掛け
-R2 ボタン == タオル真ん中掴む
-select == ハンガー掛け 最大まで上昇
-start == ハンガー展開
-*/
-
-void Toggle(int *variable, int val){
- static int data[2]={0};
-
- data[1]=data[0];
- data[0]=val;
-
- if(data[1]==0 && data[0]==1){
- *variable++;
- *variable%=2;
+ (*variable)++;
+ (*variable)%=2;
}
}
\ No newline at end of file