2019NHK_teamA
/
2019_A_ver7-5
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 25:ce789ea15628, committed 2019-10-02
- Comitter:
- yuron
- Date:
- Wed Oct 02 02:13:36 2019 +0000
- Parent:
- 24:d12bc20c01c2
- Commit message:
- aaaaaaaaaa; ;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sun Sep 29 17:01:17 2019 +0000
+++ b/main.cpp Wed Oct 02 02:13:36 2019 +0000
@@ -16,7 +16,7 @@
#define wheel_difference 100
//終了phase
-#define FINAL_PHASE 39
+#define FINAL_PHASE 50
#define RED 0
#define BLUE 1
@@ -92,7 +92,7 @@
QEI wheel_x2(PB_14, PB_13, NC, 624);
QEI wheel_y1(PB_1 , PB_15, NC, 624);
QEI wheel_y2(PA_12, PA_11, NC, 624);
-QEI arm_enc(PB_5, PB_4 , NC, 624);
+QEI arm_enc(PB_4, PB_5 , NC, 624);
//移動後n秒停止タイマー
Timer counter;
@@ -196,11 +196,6 @@
init();
init_send();
-
- //とりあえず(後で消してね)
- //phase = 22; //スタートゾーン2から
- //phase = 36; //アームアップ
- //phase = 50; //default
//起動時にゾーンを読んでからループに入る(試合中誤ってスイッチ押すのを防止)
while(1) {
@@ -228,12 +223,6 @@
i2c.write(0x30, servo_data, 1);
}
- /*
- if(start_switch == 1) {
- phase = 15;
- }
- */
-
//青ゾーン
if(zone == BLUE) {
GREEN_LED = 1;
@@ -243,14 +232,15 @@
//スタート位置へセット
case 0:
+ //スタートスイッチが押されたか
+ if(start_switch == 1) {
+ wheel_reset();
+ phase = 1;
+ }
+
//リミットが洗濯物台に触れているか
if(right_limit == 3) {
USR_LED1 = 1;
- //スタートスイッチが押されたか
- if(start_switch == 1) {
- wheel_reset();
- phase = 1;
- }
} else {
USR_LED1 = 0;
}
@@ -259,39 +249,41 @@
//回収アームを伸ばす
case 1:
counter.reset();
- //kaisyu(arm_enc.getPulses(), 2);
kaisyu_nobasu(arm_pulse, 2);
//サーボを開いておく
servo_data[0] = 0x03;
i2c.write(0x30, servo_data, 1);
break;
- //1秒停止
+ //0.5秒停止
case 2:
stop();
servo_data[0] = 0x04;
i2c.write(0x30, servo_data, 1);
counter.start();
- if(counter.read() > 1.0f) {
+ if(counter.read() > 0.5f) {
phase = 3;
wheel_reset();
}
break;
- //ちょっと後進
+ //ちょっと前進
case 3:
counter.reset();
- back(-800);
- if((y_pulse1*-1 > 800) || (y_pulse2*-1 > 800)) {
+ front(800);
+ if((y_pulse1 > 800) || (y_pulse2 > 800)) {
+ phase = 4;
+ }
+ if(front_limit == 3) {
phase = 4;
}
break;
- //1秒停止
+ //0.5秒停止
case 4:
stop();
counter.start();
- if(counter.read() > 1.0f) {
+ if(counter.read() > 0.5f) {
phase = 5;
wheel_reset();
}
@@ -303,40 +295,40 @@
kaisyu_hiku(arm_pulse, 6);
break;
- //1秒停止
+ //0.5秒停止
case 6:
stop();
counter.start();
- if(counter.read() > 1.0f) {
+ if(counter.read() > 0.5f) {
phase = 7;
wheel_reset();
}
break;
-
- //左移動
+
+ //ちょっと後進
case 7:
counter.reset();
- left(10500);
- if((x_pulse1 > 10500) || (x_pulse2 > 10500)) {
+ back(-700);
+ if((y_pulse1*-1 > 700) || (y_pulse2*-1 > 700)) {
phase = 8;
}
break;
-
- //1秒停止
+
+ //0.5秒停止
case 8:
stop();
counter.start();
- if(counter.read() > 1.0f) {
+ if(counter.read() > 0.5f) {
phase = 9;
wheel_reset();
}
break;
-
- //右旋回(180°)
+
+ //左移動
case 9:
counter.reset();
- turn_right(975);
- if(sum_pulse > 975) {
+ left(11500);
+ if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
phase = 10;
}
break;
@@ -351,12 +343,31 @@
}
break;
+ //右旋回(180°)
+ case 11:
+ counter.reset();
+ turn_right(975);
+ if(sum_pulse > 975) {
+ phase = 12;
+ }
+ break;
+
+ //0.5秒停止
+ case 12:
+ stop();
+ counter.start();
+ if(counter.read() > 0.5f) {
+ phase = 13;
+ wheel_reset();
+ }
+ break;
+
//壁に当たるまで後進
- case 11:
+ case 13:
counter.reset();
if(back_limit == 3) {
- phase = 12;
+ phase = 14;
}
else if(back_limit != 3){
true_migimae_data[0] = 0x50;
@@ -371,22 +382,22 @@
}
break;
- //1秒停止
- case 12:
+ //0.5秒停止
+ case 14:
stop();
counter.start();
- if(counter.read() > 1.0f) {
- phase = 13;
+ if(counter.read() > 0.5f) {
+ phase = 15;
wheel_reset();
}
break;
//壁に当たるまで右移動
- case 13:
+ case 15:
counter.reset();
if(right_limit == 3) {
- phase = 14;
+ phase = 16;
}
else if(right_limit != 3) {
true_migimae_data[0] = 0x40;
@@ -401,39 +412,20 @@
}
break;
- //1秒停止
- case 14:
+ //0.5秒停止
+ case 16:
stop();
counter.start();
- if(counter.read() > 1.0f) {
- phase = 15;
+ if(counter.read() > 0.5f) {
+ phase = 17;
wheel_reset();
}
break;
//排出
- case 15:
- counter.reset();
- tyokudo(arm_enc.getPulses(), 16);
- break;
-
- //1秒停止
- case 16:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 17;
- wheel_reset();
- }
- break;
-
- //前進
case 17:
counter.reset();
- front(5000);
- if((y_pulse1 > 5000) || (y_pulse2 > 5000)) {
- phase = 18;
- }
+ tyokudo(arm_enc.getPulses(), 18);
break;
//1秒停止
@@ -446,12 +438,31 @@
}
break;
+ //前進
+ case 19:
+ counter.reset();
+ front(5000);
+ if((y_pulse1 > 5000) || (y_pulse2 > 5000)) {
+ phase = 20;
+ }
+ break;
+
+ //0.5秒停止
+ case 20:
+ stop();
+ counter.start();
+ if(counter.read() > 0.5f) {
+ phase = 21;
+ wheel_reset();
+ }
+ break;
+
//壁に当たるまで右移動
- case 19:
+ case 21:
counter.reset();
if(right_limit == 3) {
- phase = 20;
+ phase = 22;
}
else if(right_limit != 3) {
true_migimae_data[0] = 0x40;
@@ -466,22 +477,22 @@
}
break;
- //1秒停止
- case 20:
+ //0.5秒停止
+ case 22:
stop();
counter.start();
- if(counter.read() > 1.0f) {
- phase = 21;
+ if(counter.read() > 0.5f) {
+ phase = 23;
wheel_reset();
}
break;
//壁に当たるまで後進
- case 21:
+ case 23:
counter.reset();
if(back_limit == 3) {
- phase = 22;
+ phase = 24;
}
else if(back_limit != 3){
true_migimae_data[0] = 0x50;
@@ -497,40 +508,21 @@
break;
//シーツ装填
- case 22:
+ case 24:
YELLOW_LED = 1;
if(start_switch == 1) {
wheel_reset();
- phase = 23;
+ phase = 25;
} else {
stop();
}
break;
//竿のラインまで前進
- case 23:
- counter.reset();
- front(20500);
- if((y_pulse1 > 20500) || (y_pulse2 > 20500)) {
- phase = 24;
- }
- break;
-
- //1秒停止
- case 24:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 25;
- wheel_reset();
- }
- break;
-
- //ちょっと左移動
case 25:
counter.reset();
- left(500);
- if((x_pulse1 > 500) || (x_pulse2 > 500)) {
+ front(21200);
+ if((y_pulse1 > 21200) || (y_pulse2 > 21200)) {
phase = 26;
}
break;
@@ -545,11 +537,11 @@
}
break;
- //90°右旋回
+ //ちょっと左移動
case 27:
counter.reset();
- turn_right(465);
- if(sum_pulse > 465) {
+ left(500);
+ if((x_pulse1 > 500) || (x_pulse2 > 500)) {
phase = 28;
}
break;
@@ -564,11 +556,30 @@
}
break;
- //壁に当たるまで前進
+ //90°右旋回
case 29:
counter.reset();
+ turn_right(465);
+ if(sum_pulse > 465) {
+ phase = 30;
+ }
+ break;
+
+ //1秒停止
+ case 30:
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
+ phase = 31;
+ wheel_reset();
+ }
+ break;
+
+ //壁に当たるまで前進
+ case 31:
+ counter.reset();
if(front_limit == 3) {
- phase = 30;
+ phase = 32;
}
else if(front_limit != 3){
true_migimae_data[0] = 0xC0;
@@ -584,26 +595,6 @@
break;
//1秒停止
- case 30:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 31;
- wheel_reset();
- }
- break;
-
- //掛けるところまで後進
- case 31:
- counter.reset();
- back(-10000);
- if((y_pulse1*-1 > 10000) || (y_pulse2*-1 > 10000)) {
- phase = 32;
- counter.start();
- }
- break;
-
- //1秒停止
case 32:
stop();
counter.start();
@@ -613,12 +604,13 @@
}
break;
- //妨害防止の右旋回
+ //掛けるところまで後進
case 33:
counter.reset();
- turn_right(30);
- if(sum_pulse > 30) {
+ back(-9500);
+ if((y_pulse1*-1 > 9500) || (y_pulse2*-1 > 9500)) {
phase = 34;
+ counter.start();
}
break;
@@ -632,17 +624,48 @@
}
break;
+ //妨害防止の右旋回
+ case 35:
+ counter.reset();
+ turn_right(30);
+ if(sum_pulse > 30) {
+ phase = 36;
+ }
+ break;
+
+ //1秒停止
+ case 36:
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
+ phase = 38;
+ wheel_reset();
+ } else {
+ right_arm_data[0] = 0xFF;
+ left_arm_data[0] = 0xFF;
+ i2c.write(0x22, right_arm_data, 1);
+ i2c.write(0x24, left_arm_data, 1);
+ wait_us(20);
+ }
+ break;
+
+ /*
//カウンターリセット
- case 35:
+ case 37:
counter.reset();
counter.start();
- phase = 36;
+ arm_up(39);
+ phase = 38;
break;
+ */
//アームアップ
- case 36:
+ case 38:
+ counter.reset();
stop();
- //3秒間リミットを読まずに無条件で上昇(チャタリングによる誤作動防止)
+ arm_up(39);
+ //1秒間リミットを読まずに無条件で上昇(チャタリングによる誤作動防止)
+ /*
if(counter.read() < 1.0f) {
right_arm_data[0] = 0xFF;
left_arm_data[0] = 0xFF;
@@ -650,49 +673,67 @@
i2c.write(0x24, left_arm_data, 1);
wait_us(20);
} else {
- arm_up(37);
+ arm_up(39);
}
+ */
break;
//カウンターリセット
- case 37:
+ case 39:
counter.reset();
- phase = 38;
+ //phase = 40;
+ phase = 42;
+ break;
+
+ //1秒停止
+ case 40:
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
+ phase = 41;
+ wheel_reset();
+ }
+ break;
+
+ //カウンターリセット
+ case 41:
+ counter.reset();
+ phase = 42;
break;
//シーツを掛ける
- case 38:
+ case 42:
counter.start();
//1秒間ファン送風
- if(counter.read() <= 2.0f) {
+ if(counter.read() <= 1.0f) {
fan_data[0] = 0xFF;
i2c.write(0x26, fan_data, 1);
i2c.write(0x28, fan_data, 1);
servo_data[0] = 0x04;
i2c.write(0x30, servo_data, 1);
}
- //1~3秒の間でサーボを開放
- else if((counter.read() > 2.0f) && (counter.read() <= 4.0f)) {
+ //1~2.75秒の間でサーボを開放
+ else if((counter.read() > 1.0f) && (counter.read() <= 2.75f)) {
fan_data[0] = 0xFF;
i2c.write(0x26, fan_data, 1);
i2c.write(0x28, fan_data, 1);
servo_data[0] = 0x03;
i2c.write(0x30, servo_data, 1);
}
- //3秒過ぎたら終わり
- else if(counter.read() > 4.0f) {
+ //2.75秒過ぎたら終わり
+ else if(counter.read() > 2.75f) {
fan_data[0] = 0x80;
i2c.write(0x26, fan_data, 1);
i2c.write(0x28, fan_data, 1);
servo_data[0] = 0x04;
i2c.write(0x30, servo_data, 1);
- phase = 39;
+ phase = FINAL_PHASE;
}
break;
//終了っ！(守衛さん風)
- case 39:
+ case FINAL_PHASE:
default:
//駆動系統OFF
all_stop();
@@ -709,14 +750,16 @@
//スタート位置へセット
case 0:
+ //スタートスイッチが押されたか
+ if(start_switch == 1) {
+ wheel_reset();
+ phase = 1;
+ }
+
//リミットが洗濯物台に触れているか
if(right_limit == 3) {
USR_LED1 = 1;
- //スタートスイッチが押されたか
- if(start_switch == 1) {
- wheel_reset();
- phase = 1;
- }
+
} else {
USR_LED1 = 0;
}
@@ -725,14 +768,13 @@
//回収アームを伸ばす
case 1:
counter.reset();
- //kaisyu(arm_enc.getPulses(), 2);
kaisyu_nobasu(arm_pulse, 2);
//サーボを開いておく
servo_data[0] = 0x03;
i2c.write(0x30, servo_data, 1);
break;
- //1秒停止
+ //0.5秒停止
case 2:
stop();
servo_data[0] = 0x04;
@@ -749,12 +791,11 @@
counter.reset();
front(800);
if((y_pulse1 > 800) || (y_pulse2 > 800)) {
- //phase = 4;
- phase = 5;
+ phase = 4;
}
break;
-
- //1秒停止
+
+ //0.5秒停止
case 4:
stop();
counter.start();
@@ -763,59 +804,58 @@
wheel_reset();
}
break;
-
+
//回収アーム引っ込める
case 5:
counter.reset();
- //kaisyu_hiku(arm_pulse, 6);
- kaisyu_hiku(arm_pulse, 7);
- break;
-
+ kaisyu_hiku(arm_pulse, 6);
+ break;
+
//左移動
- case 7:
+ case 6:
counter.reset();
left(11500);
if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
- phase = 8;
+ phase = 7;
}
break;
//1秒停止
- case 8:
+ case 7:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 9;
+ phase = 8;
wheel_reset();
}
break;
//右旋回(180°)
- case 9:
+ case 8:
counter.reset();
//turn_right(975);
turn_right(960);
if(sum_pulse > 960) {
- phase = 10;
+ phase = 9;
}
break;
- //1秒停止
- case 10:
+ //0.5秒停止
+ case 9:
stop();
counter.start();
if(counter.read() > 0.5f) {
- phase = 11;
+ phase = 10;
wheel_reset();
}
- break;
-
+ break;
+
//壁に当たるまで前進
- case 11:
+ case 10:
counter.reset();
-
+
if(front_limit == 3) {
- phase = 12;
+ phase = 11;
}
else if(front_limit != 3){
true_migimae_data[0] = 0xC0;
@@ -829,23 +869,88 @@
wait_us(20);
}
break;
-
- //1秒停止
- case 12:
+
+ //0.5秒停止
+ case 11:
stop();
counter.start();
if(counter.read() > 0.5f) {
- phase = 13;
+ phase = 12;
wheel_reset();
}
break;
//壁に当たるまで右移動
+ case 12:
+ counter.reset();
+
+ if(right_limit == 3) {
+ phase = 13;
+ }
+ else if(right_limit != 3) {
+ true_migimae_data[0] = 0x40;
+ true_migiusiro_data[0] = 0xBF;
+ true_hidarimae_data[0] = 0xBF;
+ true_hidariusiro_data[0] = 0x40;
+ i2c.write(0x10, true_migimae_data, 1, false);
+ i2c.write(0x12, true_migiusiro_data, 1, false);
+ i2c.write(0x14, true_hidarimae_data, 1, false);
+ i2c.write(0x16, true_hidariusiro_data, 1, false);
+ wait_us(20);
+ }
+ break;
+
+ //0.5秒停止
case 13:
- counter.reset();
-
+ stop();
+ counter.start();
+ if(counter.read() > 0.5f) {
+ phase = 14;
+ wheel_reset();
+ }
+ break;
+
+ //排出
+ case 14:
+ counter.reset();
+ tyokudo(arm_enc.getPulses(), 15);
+ break;
+
+ //1秒停止
+ case 15:
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
+ phase = 16;
+ wheel_reset();
+ }
+ break;
+
+ //後進
+ case 16:
+ counter.reset();
+ back(-5000);
+ if((y_pulse1*-1 > 5000) || (y_pulse2*-1 > 5000)) {
+ phase = 17;
+ }
+ break;
+
+ //0.5秒停止
+ case 17:
+ stop();
+ counter.start();
+ if(counter.read() > 0.5f) {
+ phase = 18;
+ wheel_reset();
+ }
+ break;
+
+ //壁に当たるまで右移動
+ case 18:
+ counter.reset();
+
if(right_limit == 3) {
- phase = 14;
+ phase = 19;
}
else if(right_limit != 3) {
true_migimae_data[0] = 0x40;
@@ -856,92 +961,26 @@
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
- wait_us(20);
- }
- break;
-
- //1秒停止
- case 14:
- stop();
- counter.start();
- if(counter.read() > 0.5f) {
- phase = 15;
- wheel_reset();
+ wait_us(20);
}
break;
- //排出
- case 15:
- counter.reset();
- //tyokudo(arm_enc.getPulses(), 16);
- tyokudo(arm_enc.getPulses(), 17);
- break;
-
- //1秒停止
- case 16:
+ //0.5秒停止
+ case 19:
stop();
counter.start();
- if(counter.read() > 1.0f) {
- phase = 17;
+ if(counter.read() > 0.5f) {
+ phase = 20;
wheel_reset();
}
break;
- //後進
- case 17:
- counter.reset();
- back(-5000);
- if((y_pulse1*-1 > 5000) || (y_pulse2*-1 > 5000)) {
- phase = 18;
- }
- break;
-
- //1秒停止
- case 18:
- stop();
- counter.start();
- if(counter.read() > 0.5f) {
- phase = 19;
- wheel_reset();
- }
- break;
-
- //壁に当たるまで右移動
- case 19:
+ //壁に当たるまで前進
+ case 20:
counter.reset();
-
- if(right_limit == 3) {
- phase = 20;
- }
- else if(right_limit != 3) {
- true_migimae_data[0] = 0x40;
- true_migiusiro_data[0] = 0xBF;
- true_hidarimae_data[0] = 0xBF;
- true_hidariusiro_data[0] = 0x40;
- i2c.write(0x10, true_migimae_data, 1, false);
- i2c.write(0x12, true_migiusiro_data, 1, false);
- i2c.write(0x14, true_hidarimae_data, 1, false);
- i2c.write(0x16, true_hidariusiro_data, 1, false);
- wait_us(20);
- }
- break;
-
- //1秒停止
- case 20:
- stop();
- counter.start();
- if(counter.read() > 0.5f) {
+
+ if(front_limit == 3) {
phase = 21;
- wheel_reset();
- }
- break;
-
- //壁に当たるまで前進
- case 21:
- counter.reset();
-
- if(front_limit == 3) {
- phase = 22;
}
else if(front_limit != 3){
true_migimae_data[0] = 0xC0;
@@ -957,80 +996,80 @@
break;
//シーツ装填
- case 22:
+ case 21:
YELLOW_LED = 1;
if(start_switch == 1) {
wheel_reset();
- phase = 23;
+ phase = 22;
} else {
stop();
}
break;
//竿のラインまで後進
- case 23:
+ case 22:
counter.reset();
back(-20500);
if((y_pulse1*-1 > 20500) || (y_pulse2*-1 > 20500)) {
+ phase = 23;
+ }
+ break;
+
+ //1秒停止
+ case 23:
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
phase = 24;
+ wheel_reset();
+ }
+ break;
+
+ //ちょっと左移動
+ case 24:
+ counter.reset();
+ left(500);
+ if((x_pulse1 > 500) || (x_pulse2 > 500)) {
+ phase = 25;
}
break;
//1秒停止
- case 24:
+ case 25:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 25;
- wheel_reset();
- }
- break;
-
- //ちょっと左移動
- case 25:
- counter.reset();
- left(500);
- if((x_pulse1 > 500) || (x_pulse2 > 500)) {
phase = 26;
- }
- break;
-
- //1秒停止
- case 26:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 27;
wheel_reset();
}
break;
-
+
//90°左旋回
- case 27:
+ case 26:
counter.reset();
turn_left(500);
if(sum_pulse > 500) {
- phase = 28;
+ phase = 27;
}
break;
-
+
//1秒停止
- case 28:
+ case 27:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 29;
+ phase = 28;
wheel_reset();
}
break;
-
+
//壁に当たるまで後進
- case 29:
+ case 28:
counter.reset();
-
+
if(back_limit == 3) {
- phase = 30;
- }
+ phase = 29;
+ }
else if(back_limit != 3){
true_migimae_data[0] = 0x50;
true_migiusiro_data[0] = 0x50;
@@ -1040,70 +1079,70 @@
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
- wait_us(20);
+ wait_us(20);
}
break;
-
+
//1秒停止
- case 30:
+ case 29:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 31;
+ phase = 30;
wheel_reset();
}
break;
-
+
//掛けるところまで前進
- case 31:
+ case 30:
counter.reset();
- front(11000);
- if((y_pulse1 > 11000) || (y_pulse2 > 11000)) {
- phase = 32;
+ front(10600);
+ if((y_pulse1 > 10600) || (y_pulse2 > 10600)) {
+ phase = 31;
counter.start();
}
break;
-
+
//1秒停止
- case 32:
+ case 31:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 33;
+ phase = 32;
wheel_reset();
}
break;
//妨害防止の左旋回
- case 33:
+ case 32:
counter.reset();
turn_left(30);
if(sum_pulse > 30) {
- phase = 34;
+ phase = 33;
}
break;
//1秒停止
- case 34:
+ case 33:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 35;
+ phase = 34;
wheel_reset();
}
break;
//カウンターリセット
- case 35:
+ case 34:
counter.reset();
counter.start();
- phase = 36;
+ phase = 35;
break;
-
+
//アームアップ
- case 36:
+ case 35:
stop();
- //3秒間リミットを読まずに無条件で上昇(チャタリングによる誤作動防止)
+ //1秒間リミットを読まずに無条件で上昇(チャタリングによる誤作動防止)
if(counter.read() < 1.0f) {
right_arm_data[0] = 0xFF;
left_arm_data[0] = 0xFF;
@@ -1111,20 +1150,37 @@
i2c.write(0x24, left_arm_data, 1);
wait_us(20);
} else {
- arm_up(37);
+ arm_up(36);
}
break;
+
+ //カウンターリセット
+ case 36:
+ counter.reset();
+ phase = 37;
+ break;
- //カウンターリセット
+ //1秒停止
case 37:
- counter.reset();
- phase = 38;
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
+ phase = 38;
+ wheel_reset();
+ }
break;
-
- //シーツを掛ける
+
+ //カウンターリセット
case 38:
+ counter.reset();
counter.start();
-
+ phase = 39;
+ break;
+
+ //シーツを掛ける
+ case 39:
+ counter.start();
+
//1秒間ファン送風
if(counter.read() <= 1.0f) {
fan_data[0] = 0xFF;
@@ -1133,27 +1189,27 @@
servo_data[0] = 0x04;
i2c.write(0x30, servo_data, 1);
}
- //1~3秒の間でサーボを開放
- else if((counter.read() > 1.0f) && (counter.read() <= 3.0f)) {
+ //1~2.75秒の間でサーボを開放
+ else if((counter.read() > 1.0f) && (counter.read() <= 2.75f)) {
fan_data[0] = 0xFF;
i2c.write(0x26, fan_data, 1);
i2c.write(0x28, fan_data, 1);
servo_data[0] = 0x03;
i2c.write(0x30, servo_data, 1);
}
- //3秒過ぎたら終わり
- else if(counter.read() > 3.0f) {
+ //2.75秒過ぎたら終わり
+ else if(counter.read() > 2.75f) {
fan_data[0] = 0x80;
i2c.write(0x26, fan_data, 1);
i2c.write(0x28, fan_data, 1);
servo_data[0] = 0x04;
i2c.write(0x30, servo_data, 1);
- phase = 39;
+ phase = FINAL_PHASE;
}
break;
//終了っ！(守衛さん風)
- case 39:
+ case FINAL_PHASE:
default:
//駆動系統OFF
all_stop();
@@ -1226,10 +1282,11 @@
}
void print_pulses(void) {
- //pc.printf("%d\n\r", RDATA);
+ pc.printf("%d\n\r", arm_pulse);
//pc.printf("p: %d, k_p: %d, pulse: %d\n\r", phase, kaisyu_phase, arm_pulse);
//pc.printf("X1: %d, X2: %d, Y1: %d, Y2: %d, sum: %d\n\r", abs(x_pulse1), x_pulse2, abs(y_pulse1), y_pulse2, sum_pulse);
- //pc.printf("f: %d, b: %d, r: %d, phase: %d\n\r", front_limit, back_limit, right_limit, phase);
+ //pc.printf("%d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", front_limit, back_limit, right_limit, kaisyu_mae_limit, kaisyu_usiro_limit,
+ //tyokudo_mae_limit, tyokudo_usiro_limit, right_arm_upper_limit, left_arm_upper_limit);
//pc.printf("%r: %x, l: %x\n\r", right_arm_data[0], left_arm_data[0]);
//pc.printf("limit: 0x%x, upper: 0x%x, lower: 0x%x\n\r", limit_data, upper_limit_data, lower_limit_data);
//pc.printf("x1: %d, x2: %d, y1: %d, y2: %d, phase: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2, phase);
@@ -1247,10 +1304,9 @@
emergency = 0;
/*
//終了phaseで駆動系統OFF
- if(phase >= 39) {
+ if(phase == FINAL_PHASE) {
emergency = 1;
- }
- else if(phase < 39) {
+ } else {
emergency = 0;
}
*/