Takushima Yukimasa
a
Diff: main.cpp
- Revision:
- 13:48c4a4c95e77
- Parent:
- 12:7e33e400a48d
- Child:
- 14:e6d16095ffa8
--- a/main.cpp Sun Sep 08 09:57:22 2019 +0000
+++ b/main.cpp Sun Sep 15 11:43:13 2019 +0000
@@ -2,7 +2,7 @@
/* 型定義 --------------------------------------------------------------------*/
//ゾーン対応
-//#define _RED_ZONE_
+#define _RED_ZONE_
#ifdef _RED_ZONE_
const int def_val=0;
#else
@@ -18,11 +18,12 @@
//kogo: かごをひっくり返す
//pull: 奥のリミットで引く
-//spull: 中央のリミットでつかむ(short_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;
@@ -78,6 +79,7 @@
else if(rawyaw[1]>270 && rawyaw[0]<90) rawyaw[1]-=360;
yaw-=rawyaw[0]-rawyaw[1];
+ yaw=fmod(yaw, 360);
//ゾーン対応
#ifdef _RED_ZONE_
@@ -95,7 +97,7 @@
rx = (sbdbt.RX - 64)*60/64;
//R1で停止
- if(sbdbt.R1==0) {
+ if(1) {
//メカナム基本動作
mecanum_Move(lx, ly, rx);
@@ -115,7 +117,6 @@
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);
@@ -124,7 +125,6 @@
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);
@@ -147,6 +147,8 @@
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);
@@ -206,6 +208,11 @@
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);
@@ -241,7 +248,7 @@
case 3:
//腕上げる
- i2c.Out_Set(Out_Data,0,0);
+// i2c.Out_Set(Out_Data,0,0);
if(tim.read_ms()>0) {
spull++;
tim.reset();
@@ -269,6 +276,8 @@
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);
@@ -315,12 +324,15 @@
else if(kago_f) {
// PALETTE(MAGENTA);
short_f=1;
- //腕の先を閉じておく
- i2c.Out_Set(Out_Data,1,0);
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) {
@@ -370,8 +382,11 @@
} else {
//爪開く
i2c.Out_Set(Out_Data,1,1);
- //腕上げる
- i2c.Out_Set(Out_Data,0,0);
+
+ //腕おろす
+ if(sbdbt.R1) i2c.Out_Set(Out_Data,0,1);
+ //腕あげる
+ else i2c.Out_Set(Out_Data,0,0);
//腕真ん中で停止
if(short_f) {
@@ -385,9 +400,32 @@
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);
+ else MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
MD_SET_PWM (MD_Data,4,30);
- } else {
+ }
+ //赤ゾーン
+ else if(yaw<=45 || 315<=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(135<=yaw && yaw<=225) {
+ 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);
}
@@ -445,16 +483,6 @@
slide_start = (In_Data+3)->in_data;
slide_stop = (In_Data+0)->in_data;
- //半自動のデバック
-// 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("%d %d",keep_tim[0].read_ms(),keep_f[0]);
- pc.printf("\n\r");
-
/* ----------------ボタン系--------------- */
//角度リセット
if(sbdbt.L1) {
@@ -510,7 +538,17 @@
} // if(sbdbt.State_check())
- else PALETTE(BLACK);
+
+ //半自動のデバック
+// 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("%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);
+ pc.printf("\n\r");
} // while(1)
} // int main()