2019NHK_teamA
2019年10月02日AM11:14現在のもの(青ゾーンは変更なし)
Diff: main.cpp
- Revision:
- 23:1e4d7540715f
- Parent:
- 22:5682246f9409
- Child:
- 24:d12bc20c01c2
--- a/main.cpp Wed Sep 25 02:07:26 2019 +0000
+++ b/main.cpp Sat Sep 28 01:24:03 2019 +0000
@@ -15,6 +15,9 @@
//直進補正の為の前後・左右の回転差の許容値
#define wheel_difference 100
+//終了phase
+#define FINAL_PHASE 39
+
#define RED 0
#define BLUE 1
@@ -94,7 +97,7 @@
Timer counter;
//エンコーダ値格納変数
-int x_pulse1, x_pulse2, y_pulse1, y_pulse2, sum_pulse;
+int x_pulse1, x_pulse2, y_pulse1, y_pulse2, sum_pulse, arm_pulse;
//操作の段階変数
unsigned int phase = 0;
@@ -169,6 +172,8 @@
void read_limit(void);
void wheel_reset(void);
void kaisyu(int pulse, int next_phase);
+void kaisyu_nobasu(int pulse, int next_phase);
+void kaisyu_hiku(int pulse, int next_phase);
void tyokudo(int pulse, int next_phase);
void arm_up(int next_phase);
void front(int target);
@@ -192,10 +197,9 @@
init_send();
//とりあえず(後で消してね)
- //zone = BLUE;
- //phase = 16;
- //phase = 23;
- phase = 50;
+ //phase = 22; //スタートゾーン2から
+ //phase = 36; //アームアップ
+ //phase = 50; //default
//起動時にゾーンを読んでからループに入る(試合中誤ってスイッチ押すのを防止)
while(1) {
@@ -210,7 +214,7 @@
while(1) {
get_pulses();
- print_pulses();
+ //print_pulses();
get_emergency();
read_limit();
@@ -223,27 +227,17 @@
i2c.write(0x30, servo_data, 1);
}
+ /*
if(start_switch == 1) {
- //phase = 31;
- 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);
- } else {
- right_arm_data[0] = 0x80;
- left_arm_data[0] = 0x80;
- i2c.write(0x22, right_arm_data, 1);
- i2c.write(0x24, left_arm_data, 1);
- wait_us(20);
+ phase = 39;
}
+ */
- /*
//青ゾーン
if(zone == BLUE) {
GREEN_LED = 1;
RED_LED = 0;
-
+
switch(phase) {
//スタート位置へセット
@@ -261,9 +255,12 @@
}
break;
- //回収
+ //回収アームを伸ばす
case 1:
- kaisyu(arm_enc.getPulses(), 2);
+ counter.reset();
+ //kaisyu(arm_enc.getPulses(), 2);
+ kaisyu_nobasu(arm_pulse, 2);
+ //サーボを開いておく
servo_data[0] = 0x03;
i2c.write(0x30, servo_data, 1);
break;
@@ -280,11 +277,11 @@
}
break;
- //左移動
+ //ちょっと後進
case 3:
counter.reset();
- left(11500);
- if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
+ back(-800);
+ if((y_pulse1*-1 > 800) || (y_pulse2*-1 > 800)) {
phase = 4;
}
break;
@@ -299,13 +296,10 @@
}
break;
- //右旋回(180°)
+ //回収アーム引っ込める
case 5:
counter.reset();
- turn_right(975);
- if(sum_pulse > 975) {
- phase = 6;
- }
+ kaisyu_hiku(arm_pulse, 6);
break;
//1秒停止
@@ -318,24 +312,13 @@
}
break;
- //壁に当たるまで右移動
+ //左移動
case 7:
- counter.reset();
-
- if(right_limit == 3) {
+ counter.reset();
+ left(11500);
+ if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
phase = 8;
}
- 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秒停止
@@ -348,10 +331,13 @@
}
break;
- //排出
+ //右旋回(180°)
case 9:
counter.reset();
- tyokudo(arm_enc.getPulses(), 10);
+ turn_right(975);
+ if(sum_pulse > 975) {
+ phase = 10;
+ }
break;
//1秒停止
@@ -364,13 +350,24 @@
}
break;
- //前進
+ //壁に当たるまで後進
case 11:
counter.reset();
- front(5000);
- if((y_pulse1 > 5000) || (y_pulse2 > 5000)) {
+
+ if(back_limit == 3) {
phase = 12;
}
+ else if(back_limit != 3){
+ true_migimae_data[0] = 0x50;
+ true_migiusiro_data[0] = 0x50;
+ true_hidarimae_data[0] = 0x50;
+ true_hidariusiro_data[0] = 0x50;
+ 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秒停止
@@ -386,7 +383,7 @@
//壁に当たるまで右移動
case 13:
counter.reset();
-
+
if(right_limit == 3) {
phase = 14;
}
@@ -399,10 +396,10 @@
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 14:
stop();
@@ -412,13 +409,78 @@
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;
+ }
+ break;
+
+ //1秒停止
+ case 18:
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
+ phase = 19;
+ wheel_reset();
+ }
+ break;
+
+ //壁に当たるまで右移動
+ case 19:
+ 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() > 1.0f) {
+ phase = 21;
+ wheel_reset();
+ }
+ break;
+
+ //壁に当たるまで後進
+ case 21:
+ counter.reset();
+
if(back_limit == 3) {
- phase = 16;
+ phase = 22;
}
else if(back_limit != 3){
true_migimae_data[0] = 0x50;
@@ -434,85 +496,78 @@
break;
//シーツ装填
- case 16:
+ case 22:
if(start_switch == 1) {
wheel_reset();
- phase = 17;
+ phase = 23;
} else {
stop();
}
break;
//竿のラインまで前進
- case 17:
+ case 23:
counter.reset();
- front(22000);
- if((y_pulse1 > 22000) || (y_pulse2 > 22000)) {
- phase = 18;
+ 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)) {
+ phase = 26;
}
break;
//1秒停止
- case 18:
+ case 26:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 19;
- wheel_reset();
- }
- break;
-
- //ちょっと左移動
- case 19:
- counter.reset();
- left(500);
- if((x_pulse1 > 800) || (x_pulse2 > 800)) {
- phase = 20;
- }
- break;
-
- //1秒停止
- case 20:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 21;
+ phase = 27;
wheel_reset();
}
break;
-
- //90°右旋回
- case 21:
+
+ //90°右旋回
+ case 27:
counter.reset();
turn_right(465);
if(sum_pulse > 465) {
- phase = 22;
+ phase = 28;
}
break;
-
+
//1秒停止
- case 22:
+ case 28:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 23;
+ phase = 29;
wheel_reset();
}
break;
-
- //カウンターリセット
- case 23:
+
+ //壁に当たるまで前進
+ case 29:
counter.reset();
- counter.start();
- phase = 24;
- break;
-
- //壁に当たるまで前進
- case 24:
if(front_limit == 3) {
- counter.reset();
- phase = 25;
- }
+ phase = 30;
+ }
else if(front_limit != 3){
true_migimae_data[0] = 0xC0;
true_migiusiro_data[0] = 0xC0;
@@ -522,93 +577,93 @@
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 25:
+ case 30:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 26;
+ phase = 31;
wheel_reset();
}
break;
-
+
//掛けるところまで後進
- case 26:
+ case 31:
counter.reset();
back(-10000);
if((y_pulse1*-1 > 10000) || (y_pulse2*-1 > 10000)) {
- phase = 27;
+ phase = 32;
counter.start();
}
break;
-
+
//1秒停止
- case 27:
+ case 32:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 28;
+ phase = 33;
wheel_reset();
}
break;
//妨害防止の右旋回
- case 28:
+ case 33:
counter.reset();
turn_right(30);
if(sum_pulse > 30) {
- phase = 29;
+ phase = 34;
}
break;
//1秒停止
- case 29:
+ case 34:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 30;
+ phase = 35;
wheel_reset();
}
break;
//カウンターリセット
- case 30:
+ case 35:
counter.reset();
counter.start();
- phase = 31;
+ phase = 36;
break;
-
+
//アームアップ
- case 31:
+ case 36:
stop();
//3秒間リミットを読まずに無条件で上昇(チャタリングによる誤作動防止)
- if(counter.read() < 3.0f) {
+ if(counter.read() < 1.0f) {
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);
} else {
- arm_up(32);
+ arm_up(37);
}
break;
-
+
//カウンターリセット
- case 32:
+ case 37:
counter.reset();
- phase = 33;
+ phase = 38;
break;
-
+
//シーツを掛ける
- case 33:
+ case 38:
counter.start();
-
+
//1秒間ファン送風
- if(counter.read() <= 1.0f) {
+ if(counter.read() <= 2.0f) {
fan_data[0] = 0xFF;
i2c.write(0x26, fan_data, 1);
i2c.write(0x28, fan_data, 1);
@@ -616,7 +671,7 @@
i2c.write(0x30, servo_data, 1);
}
//1~3秒の間でサーボを開放
- else if((counter.read() > 1.0f) && (counter.read() <= 3.0f)) {
+ else if((counter.read() > 2.0f) && (counter.read() <= 4.0f)) {
fan_data[0] = 0xFF;
i2c.write(0x26, fan_data, 1);
i2c.write(0x28, fan_data, 1);
@@ -624,21 +679,20 @@
i2c.write(0x30, servo_data, 1);
}
//3秒過ぎたら終わり
- else if(counter.read() > 3.0f) {
+ else if(counter.read() > 4.0f) {
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 = 34;
+ phase = 39;
}
break;
//終了っ！(守衛さん風)
- case 34:
+ case 39:
default:
//駆動系統OFF
- emergency = 0;
all_stop();
break;
}
@@ -666,9 +720,12 @@
}
break;
- //回収
+ //回収アームを伸ばす
case 1:
- kaisyu(arm_enc.getPulses(), 2);
+ counter.reset();
+ //kaisyu(arm_enc.getPulses(), 2);
+ kaisyu_nobasu(arm_pulse, 2);
+ //サーボを開いておく
servo_data[0] = 0x03;
i2c.write(0x30, servo_data, 1);
break;
@@ -685,15 +742,15 @@
}
break;
- //左移動
+ //ちょっと前進
case 3:
counter.reset();
- left(11500);
- if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
+ front(800);
+ if((y_pulse1 > 800) || (y_pulse2 > 800)) {
phase = 4;
}
break;
-
+
//1秒停止
case 4:
stop();
@@ -703,16 +760,13 @@
wheel_reset();
}
break;
-
- //右旋回(180°)
+
+ //回収アーム引っ込める
case 5:
counter.reset();
- turn_right(975);
- if(sum_pulse > 975) {
- phase = 6;
- }
+ kaisyu_hiku(arm_pulse, 6);
break;
-
+
//1秒停止
case 6:
stop();
@@ -721,14 +775,82 @@
phase = 7;
wheel_reset();
}
+ break;
+
+ //左移動
+ case 7:
+ counter.reset();
+ left(11500);
+ if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
+ phase = 8;
+ }
+ break;
+
+ //1秒停止
+ case 8:
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
+ phase = 9;
+ wheel_reset();
+ }
+ break;
+
+ //右旋回(180°)
+ case 9:
+ counter.reset();
+ turn_right(975);
+ if(sum_pulse > 975) {
+ phase = 10;
+ }
+ break;
+
+ //1秒停止
+ case 10:
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
+ phase = 11;
+ wheel_reset();
+ }
+ break;
+
+ //壁に当たるまで前進
+ case 11:
+ counter.reset();
+
+ if(front_limit == 3) {
+ phase = 12;
+ }
+ else if(front_limit != 3){
+ true_migimae_data[0] = 0xC0;
+ true_migiusiro_data[0] = 0xC0;
+ true_hidarimae_data[0] = 0xC0;
+ true_hidariusiro_data[0] = 0xC0;
+ 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 12:
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
+ phase = 13;
+ wheel_reset();
+ }
break;
//壁に当たるまで右移動
- case 7:
+ case 13:
counter.reset();
if(right_limit == 3) {
- phase = 8;
+ phase = 14;
}
else if(right_limit != 3) {
true_migimae_data[0] = 0x40;
@@ -744,56 +866,56 @@
break;
//1秒停止
- case 8:
+ case 14:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 9;
+ phase = 15;
wheel_reset();
}
break;
//排出
- case 9:
+ case 15:
counter.reset();
- tyokudo(arm_enc.getPulses(), 10);
+ tyokudo(arm_enc.getPulses(), 16);
break;
//1秒停止
- case 10:
+ case 16:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 11;
+ phase = 17;
wheel_reset();
}
break;
//後進
- case 11:
+ case 17:
counter.reset();
back(-5000);
if((y_pulse1*-1 > 5000) || (y_pulse2*-1 > 5000)) {
- phase = 12;
+ phase = 18;
}
break;
//1秒停止
- case 12:
+ case 18:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 13;
+ phase = 19;
wheel_reset();
}
break;
//壁に当たるまで右移動
- case 13:
+ case 19:
counter.reset();
if(right_limit == 3) {
- phase = 14;
+ phase = 20;
}
else if(right_limit != 3) {
true_migimae_data[0] = 0x40;
@@ -809,21 +931,21 @@
break;
//1秒停止
- case 14:
+ case 20:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 15;
+ phase = 21;
wheel_reset();
}
break;
//壁に当たるまで前進
- case 15:
+ case 21:
counter.reset();
if(front_limit == 3) {
- phase = 16;
+ phase = 22;
}
else if(front_limit != 3){
true_migimae_data[0] = 0xC0;
@@ -839,84 +961,79 @@
break;
//シーツ装填
- case 16:
+ case 22:
if(start_switch == 1) {
wheel_reset();
- phase = 17;
+ phase = 23;
} else {
stop();
}
break;
//竿のラインまで後進
- case 17:
+ case 23:
counter.reset();
- back(-22000);
- if((y_pulse1*-1 > 22000) || (y_pulse2*-1 > 22000)) {
- phase = 18;
+ back(-20500);
+ if((y_pulse1*-1 > 20500) || (y_pulse2*-1 > 20500)) {
+ phase = 24;
}
break;
//1秒停止
- case 18:
+ case 24:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 19;
+ phase = 25;
wheel_reset();
}
break;
//ちょっと左移動
- case 19:
+ case 25:
counter.reset();
left(500);
if((x_pulse1 > 500) || (x_pulse2 > 500)) {
- phase = 20;
+ phase = 26;
}
break;
//1秒停止
- case 20:
+ case 26:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 21;
+ phase = 27;
wheel_reset();
}
break;
//90°左旋回
- case 21:
+ case 27:
counter.reset();
- turn_left(465);
- if(sum_pulse > 465) {
- phase = 22;
+ //turn_left(465);
+ turn_left(485);
+ if(sum_pulse > 485) {
+ phase = 28;
}
break;
//1秒停止
- case 22:
+ case 28:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 23;
+ phase = 29;
wheel_reset();
}
break;
- //カウンターリセット
- case 23:
+ //壁に当たるまで後進
+ case 29:
counter.reset();
- counter.start();
- phase = 24;
- break;
- //壁に当たるまで後進
- case 24:
if(back_limit == 3) {
- counter.reset();
- phase = 25;
+ phase = 30;
}
else if(back_limit != 3){
true_migimae_data[0] = 0x50;
@@ -932,88 +1049,88 @@
break;
//1秒停止
- case 25:
+ case 30:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 26;
+ phase = 31;
wheel_reset();
}
break;
//掛けるところまで前進
- case 26:
+ case 31:
counter.reset();
front(10000);
if((y_pulse1 > 10000) || (y_pulse2 > 10000)) {
- phase = 27;
+ phase = 32;
counter.start();
}
break;
//1秒停止
- case 27:
+ case 32:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 28;
+ phase = 33;
wheel_reset();
}
break;
//妨害防止の左旋回
- case 28:
+ case 33:
counter.reset();
turn_left(30);
if(sum_pulse > 30) {
- phase = 29;
+ phase = 34;
}
break;
//1秒停止
- case 29:
+ case 34:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 30;
+ phase = 35;
wheel_reset();
}
break;
//カウンターリセット
- case 30:
+ case 35:
counter.reset();
counter.start();
- phase = 31;
+ phase = 36;
break;
//アームアップ
- case 31:
+ case 36:
stop();
//3秒間リミットを読まずに無条件で上昇(チャタリングによる誤作動防止)
- if(counter.read() < 3.0f) {
+ if(counter.read() < 1.0f) {
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);
} else {
- arm_up(32);
+ arm_up(37);
}
break;
//カウンターリセット
- case 32:
+ case 37:
counter.reset();
- phase = 33;
+ phase = 38;
break;
//シーツを掛ける
- case 33:
+ case 38:
counter.start();
//1秒間ファン送風
- if(counter.read() <= 1.0f) {
+ if(counter.read() <= 2.0f) {
fan_data[0] = 0xFF;
i2c.write(0x26, fan_data, 1);
i2c.write(0x28, fan_data, 1);
@@ -1021,7 +1138,7 @@
i2c.write(0x30, servo_data, 1);
}
//1~3秒の間でサーボを開放
- else if((counter.read() > 1.0f) && (counter.read() <= 3.0f)) {
+ else if((counter.read() > 2.0f) && (counter.read() <= 4.0f)) {
fan_data[0] = 0xFF;
i2c.write(0x26, fan_data, 1);
i2c.write(0x28, fan_data, 1);
@@ -1029,26 +1146,24 @@
i2c.write(0x30, servo_data, 1);
}
//3秒過ぎたら終わり
- else if(counter.read() > 3.0f) {
+ else if(counter.read() > 4.0f) {
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 = 34;
+ phase = 39;
}
break;
//終了っ！(守衛さん風)
- case 34:
+ case 39:
default:
//駆動系統OFF
- emergency = 0;
all_stop();
break;
}
}
- */
}
}
@@ -1109,17 +1224,17 @@
y_pulse1 = wheel_y1.getPulses();
y_pulse2 = wheel_y2.getPulses();
sum_pulse = (abs(x_pulse1) + abs(x_pulse2) + abs(y_pulse1) + abs(y_pulse2)) / 4;
+ arm_pulse = arm_enc.getPulses();
}
void print_pulses(void) {
-
- 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("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("%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);
//pc.printf("RF: %x, RB: %x, LF: %x, LB: %x, phase: %d\n\r", true_migimae_data[0], true_migiusiro_data[0], true_hidarimae_data[0], true_hidariusiro_data[0], phase);
- //pc.printf("RF: %x, RB: %x, LF: %x, LB: %x, phase: %d\n\r", migimae_data[0], migiusiro_data[0], hidarimae_data[0], hidariusiro_data[0], phase);
}
void get_emergency(void) {
@@ -1131,6 +1246,14 @@
else if(RDATA == '9'){
myled = 0.2;
emergency = 0;
+
+ //終了phaseで駆動系統OFF
+ if(phase >= 39) {
+ emergency = 1;
+ }
+ else if(phase < 39) {
+ emergency = 0;
+ }
}
}
@@ -1361,7 +1484,6 @@
break;
case 1:
- USR_LED3 = 1;
//前進->停止->後進
//3600pulseまで低速前進
if(pulse < 3600) {
@@ -1382,6 +1504,11 @@
//後ろのリミットが押されたら強制停止
if(kaisyu_usiro_limit == 1) {
arm_motor[0] = 0x80;
+ //1秒待ってから引っ込める
+ counter.start();
+ if(counter.read() > 1.0f) {
+ kaisyu_phase = 2;
+ }
}
break;
@@ -1426,6 +1553,89 @@
i2c.write(0x18, arm_motor, 1);
}
+void kaisyu_nobasu(int pulse, int next_phase) {
+
+ switch (kaisyu_phase) {
+ case 0:
+ //前進->減速
+ //3000pulseまで高速前進
+ if(pulse < 3000) {
+ arm_motor[0] = 0xFF;
+ }
+ //3000pulse超えたら低速前進
+ else if(pulse >= 3000) {
+ arm_motor[0] = 0xB3;
+ kaisyu_phase = 1;
+ }
+ break;
+
+ case 1:
+ //前進->停止->後進
+ //3600pulseまで低速前進
+ if(pulse < 3600) {
+ arm_motor[0] = 0xB3;
+ }
+ //3600pulse超えたら停止
+ else if(pulse >= 3600) {
+ arm_motor[0] = 0x80;
+ phase = next_phase;
+ kaisyu_phase = 2;
+ }
+ //後ろのリミットが押されたら強制停止
+ if(kaisyu_usiro_limit == 1) {
+ arm_motor[0] = 0x80;
+ phase = next_phase;
+ kaisyu_phase = 2;
+ }
+ break;
+
+ default:
+ arm_motor[0] = 0x80;
+ break;
+ }
+ //回収MDへ書き込み
+ i2c.write(0x18, arm_motor, 1);
+}
+
+void kaisyu_hiku(int pulse, int next_phase) {
+
+ switch(kaisyu_phase) {
+ case 2:
+ //後進->減速
+ //500pulseまで高速後進
+ if(pulse > 500) {
+ arm_motor[0] = 0x00;
+ }
+ //500pulse以下になったら低速後進
+ else if(pulse <= 500) {
+ arm_motor[0] = 0x4C;
+ kaisyu_phase = 3;
+ }
+ break;
+
+ case 3:
+ //後進->停止
+ //リミット押されるまで低速後進
+ if(pulse <= 500) {
+ arm_motor[0] = 0x4C;
+ }
+
+ //リミット押されたら停止
+ if(kaisyu_mae_limit == 1) {
+ arm_motor[0] = 0x80;
+ kaisyu_phase = 4;
+ phase = next_phase;
+ }
+ break;
+
+ default:
+ arm_motor[0] = 0x80;
+ break;
+ }
+ //回収MDへ書き込み
+ i2c.write(0x18, arm_motor, 1);
+}
+
void tyokudo(int pulse, int next_phase) {
switch(tyokudo_phase) {
@@ -1825,12 +2035,11 @@
//制御量の最小、最大
//左進(目標まで達していない)
if((x_pulse1 < target) && (x_pulse2 < target)) {
- left_migimae.setOutputLimits(0xEC, 0xED);
- //left_migiusiro.setOutputLimits(0x7A, 0x7B);
- left_migiusiro.setOutputLimits(0x77, 0x78);
- left_hidarimae.setOutputLimits(0x7A, 0x7B);
- //left_hidarimae.setOutputLimits(0x77, 0x78);
- left_hidariusiro.setOutputLimits(0xFE, 0xFF);
+ left_migimae.setOutputLimits(0xEC, 0xED); //0xFFに近いほうが速い
+ left_migiusiro.setOutputLimits(0x7A, 0x7B); //0x7Bに近いほうが速い
+ //left_hidarimae.setOutputLimits(0x6B, 0x6C); //0x7Bに近いほうが速い
+ left_hidarimae.setOutputLimits(0x6E, 0x6F);
+ left_hidariusiro.setOutputLimits(0xFE, 0xFF); //0xFFに近いほうが速い
}
//停止(目標より行き過ぎ)
else if((x_pulse1 > target) && (x_pulse2 > target)) {