関東甲信越地区大会でのプログラム 青ゾーンで回収時にアームを引っ込めないバグがまだあるので注意
Diff: main.cpp
- Revision:
- 25:ce789ea15628
- Parent:
- 24:d12bc20c01c2
- Child:
- 26:81346a21d301
--- 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;
}
*/