2019NHK_teamA
/
2019_A_ver7-3
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 23:1e4d7540715f, committed 2019-09-28
- Comitter:
- yuron
- Date:
- Sat Sep 28 01:24:03 2019 +0000
- Parent:
- 22:5682246f9409
- Commit message:
- aaaaaaaaa;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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)) {