Takushima Yukimasa
a
Diff: main.cpp
- Revision:
- 14:e6d16095ffa8
- Parent:
- 13:48c4a4c95e77
- Child:
- 15:68720ff6bbc9
- Child:
- 18:d6be6154f9aa
--- a/main.cpp Sun Sep 15 11:43:13 2019 +0000
+++ b/main.cpp Sat Sep 21 01:06:15 2019 +0000
@@ -1,18 +1,18 @@
#include "main.h"
/* 型定義 --------------------------------------------------------------------*/
-//ゾーン対応
-#define _RED_ZONE_
-#ifdef _RED_ZONE_
-const int def_val=0;
-#else
-const int def_val=180;
-#endif
/* 関数宣言 ------------------------------------------------------------------*/
/* 変数宣言 ------------------------------------------------------------------*/
+//0: 赤
+//1: 青
+int zone=0;
+
+//デフォルトの角度
+int def_val=0;
+
//コントローラーの状態を保存するため
int once=0;
@@ -27,20 +27,23 @@
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};
-//yaw: 実際に使う角度
-double yaw=def_val;
+//実際に使う角度
+double yaw;
//[0]: 今回の値
//[1]: 前回の値
double rawyaw[2];
//Turn_val: 補正の目標値
-double Turn_val=def_val;
+double Turn_val;
//short_lim: 腕中央
//max_lim: 腕奥側
@@ -58,6 +61,22 @@
/*----------------------------------- 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();
@@ -79,70 +98,318 @@
else if(rawyaw[1]>270 && rawyaw[0]<90) rawyaw[1]-=360;
yaw-=rawyaw[0]-rawyaw[1];
- yaw=fmod(yaw, 360);
+ yaw=fmod(yaw, 360.0);
//ゾーン対応
-#ifdef _RED_ZONE_
- PALETTE(RED);
-#else
- PALETTE(BLUE);
-#endif
+ if(zone==0) PALETTE(RED);
+ else PALETTE(BLUE);
+
+ //リミット
+ 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;
+
+ //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();
- if(sbdbt.State_check()) {
- //リミット
- for(int i=0; i<7; i++) i2c.In(In_Data,i);
+ 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);
+ }
- 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;
+ //タオルつかむ 四角
+ 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;
- //R1で停止
- if(1) {
- //メカナム基本動作
- mecanum_Move(lx, ly, rx);
+ 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;
- //旋回している間タイマーをリセット
- if(sbdbt.RX != 64) drift_tim.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(drift_tim.read_ms()<1000) Turn_val=yaw;
+ //真ん中のリミットより前に腕があった場合にバックする
+ 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;
- //角度補正
- AngleCorrection(yaw, Turn_val);
+ 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;
- /*---ハンガーかけるやつ----------------------------------------------*/
- //上方向
- if(!DOWNkey && ((UPkey || keep_f[0]) && limit_up == 0)) {
- //一定秒以上押されたら保持
- if(keep_tim[0].read_ms()>500) keep_f[0]=1;
- //下方向の保持を解除
- keep_f[1]=0;
+ case 2:
+ //爪閉じる
+ i2c.Out_Set(Out_Data,1,0);
+ if(tim.read_ms()>0) {
+ spull++;
+ tim.reset();
+ }
+ break;
- MD_SET_DRIVE(MD_Data, 5,MD_FORWARD);
- MD_SET_PWM(MD_Data, 5,100);
+ 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;
+ } //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;
- 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();
+ 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;
- MD_SET_PWM(MD_Data,5,0);
- MD_SET_DRIVE(MD_Data, 5,MD_BRAKE);
+ 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:
//爪開く
@@ -154,7 +421,7 @@
MD_SET_PWM (MD_Data,4,60);
if(max_lim==1) {
- grab++;
+ disturb++;
tim.reset();
}
break;
@@ -167,72 +434,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;
@@ -241,170 +443,61 @@
//爪閉じる
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();
+ //赤ゾーン
+ 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 2:
- if(tim.read_ms()>0) {
- pull++;
- 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;
-
- 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;
+ } //if(disturb_f)
- switch(kago) {
- //腕を前に動かす
- case 0:
- //爪閉じる
- i2c.Out_Set(Out_Data,1,0);
- //腕上げる
- i2c.Out_Set(Out_Data,0,0);
+ //腕真ん中で停止
+ else if(short_f) {
+ //爪開く
+ i2c.Out_Set(Out_Data,1,1);
- 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);
+ 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;
- //かご回転
- 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;
- }
+ 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(sbdbt.R1) i2c.Out_Set(Out_Data,0,1);
- //腕あげる
- else i2c.Out_Set(Out_Data,0,0);
- //腕真ん中で停止
- if(short_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(yaw<=45 || 315<=yaw) {
+ 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);
@@ -414,7 +507,7 @@
}
}
//青ゾーン
- else if(135<=yaw && yaw<=225) {
+ 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);
@@ -437,117 +530,123 @@
pull=0;
grab=0;
spull=0;
- tim.reset();
- }
- }
- //非常停止
- 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.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);
-
- //エアー解放
- 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);
-
- /* ----------- りみっと --------------- */
- 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;
+ }
+ //非常停止
+ else {
+ for(int i=0; i<8; i++) {
+ MD_SET_DRIVE(MD_Data,i,MD_BRAKE);
+ MD_SET_PWM (MD_Data,i,0);
}
- //かごを倒す
- 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++;
- }
+ }
+
+ /* --------------電磁弁系-------------------- */
+ /*タオル掛け*/
+ 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);
+
+ //エアー解放
+ 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);
+
+ /* ----------- りみっと --------------- */
+ 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) {
+ if(once==0) {
+ kago_f++;
+ kago_f%=2;
+ once++;
}
- //倒すを引いて動かす
- else if(CIRCLE) {
- if(once==0) {
- pull_f++;
- pull_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.START) {
+ if(once==0) {
+ unfold_f++;
+ unfold_f%=2;
+ once++;
}
- //タオル展開用
- else if(sbdbt.SELECT) {
- if(once==0) {
- release_f++;
- release_f%=2;
- once++;
- }
- } else once=0;
-
-
- } // if(sbdbt.State_check())
+ }
+ //タオル展開用
+ 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("lx %d ly %d rx %d",lx, ly, rx);
+// 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("\n\r");
} // while(1)
} // int main()