2019NHK_teamA
/
2019_A_ver8
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.
Diff: main.cpp
- Revision:
- 28:e95db716197d
- Parent:
- 27:4f2fc7172b31
--- a/main.cpp Sat Oct 19 01:10:39 2019 +0000
+++ b/main.cpp Wed Oct 23 13:52:52 2019 +0000
@@ -18,8 +18,11 @@
#define RED 0
#define BLUE 1
-#define wind_time1 1.00f
-#define wind_time2 1.25f
+//#define wind_time1 1.00f
+//#define wind_time2 1.75f
+
+#define wind_time1 0.30f
+#define wind_time2 1.50f
//PID Gain of wheels(Kp, Ti, Td, control cycle)
//前進
@@ -107,6 +110,8 @@
unsigned int start_zone = 1;
bool zone = RED;
+bool stop_flag[30] = {0};
+
//i2c送信データ変数
char init_send_data[1];
char migimae_data[1], migiusiro_data[1], hidarimae_data[1], hidariusiro_data[1];
@@ -164,6 +169,8 @@
int masked_tyokudo_usiro_limit_data = 0b11111111;
//関数のプロトタイプ宣言
+void red_move(void);
+void blue_move(void);
void init(void);
void init_send(void);
void get(void);
@@ -176,6 +183,8 @@
void kaisyu_nobasu(int next_phase);
void kaisyu_hiku(int next_phase);
void tyokudo(int pulse, int next_phase);
+void tyokudo_nobasu(int next_phase);
+void tyokudo_hiku(int next_phase);
void arm_up(int next_phase);
void fan_on(float first_wind_time, float second_wind_time, int next_phase);
void front(int target);
@@ -197,7 +206,7 @@
init();
init_send();
- //後で消せ俺さん！！
+ //消し忘れ注意!!
//phase = 50;
//起動時にゾーンを読んでからループに入る(試合中誤ってスイッチ押すのを防止)
@@ -211,12 +220,12 @@
}
while(1) {
-
+
get_pulses();
//print_pulses();
get_emergency();
read_limit();
-
+
//move_servo_with_using_onboard-switch
if(USR_SWITCH == 0) {
servo_data[0] = 0x03;
@@ -226,15 +235,15 @@
i2c.write(0x30, servo_data, 1);
}
+ //消し忘れ注意!!
/*
if(start_switch == 1) {
- //phase = 37;
+ counter.reset();
if(zone == RED) {
phase = 35;
- //phase = 37;
}
else if(zone == BLUE) {
- phase = 37;
+ phase = 39;
}
}
*/
@@ -243,946 +252,1051 @@
if(zone == BLUE) {
GREEN_LED = 1;
RED_LED = 0;
-
- switch(phase) {
-
- //スタート位置へセット
- case 0:
- //スタートスイッチが押されたか
- if(start_switch == 1) {
- wheel_reset();
- phase = 1;
- }
-
- //リミットが洗濯物台に触れているか
- if(right_limit == 3) {
- USR_LED1 = 1;
- } else {
- USR_LED1 = 0;
- }
- break;
-
- //回収アームを伸ばす
- case 1:
- counter.reset();
- kaisyu_nobasu(2);
- //サーボを開いておく
- servo_data[0] = 0x03;
- i2c.write(0x30, servo_data, 1);
- break;
-
- //1秒停止
- case 2:
- stop();
- servo_data[0] = 0x04;
- i2c.write(0x30, servo_data, 1);
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 3;
- wheel_reset();
- }
- break;
-
- //ちょっと前進
- case 3:
- counter.reset();
- front(700);
- if((y_pulse1 > 700) || (y_pulse2 > 700)) {
- phase = 4;
- }
- if(front_limit == 3) {
- phase = 4;
- }
- break;
-
- //0.5秒停止
- case 4:
- stop();
- counter.start();
- if(counter.read() > 0.5f) {
- phase = 5;
- wheel_reset();
- }
- break;
-
- //回収アーム引っ込める
- case 5:
- USR_LED3 = 1;
- counter.reset();
- kaisyu_hiku(6);
- break;
-
- //0.5秒停止
- case 6:
- stop();
- USR_LED4 = 1;
- counter.start();
- if(counter.read() > 0.5f) {
- phase = 7;
- wheel_reset();
- }
- break;
-
- //ちょっと後進
- case 7:
- counter.reset();
- back(-700);
- if((y_pulse1*-1 > 700) || (y_pulse2*-1 > 700)) {
- phase = 8;
- }
- break;
-
- //0.5秒停止
- case 8:
- stop();
- counter.start();
- if(counter.read() > 0.5f) {
- phase = 9;
- wheel_reset();
- }
- break;
-
- //左移動
- case 9:
- counter.reset();
- left(11500);
- if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
- phase = 10;
- }
- break;
-
- //1秒停止
- case 10:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 11;
- wheel_reset();
- }
- 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 13:
- counter.reset();
-
- if(back_limit == 3) {
- phase = 14;
- }
- 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;
-
- //0.5秒停止
- case 14:
- stop();
- counter.start();
- if(counter.read() > 0.5f) {
- phase = 15;
- wheel_reset();
- }
- break;
-
- //壁に当たるまで右移動
- case 15:
- counter.reset();
-
- if(right_limit == 3) {
- phase = 16;
- }
- 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 16:
- stop();
- counter.start();
- if(counter.read() > 0.5f) {
- phase = 17;
- wheel_reset();
- }
- break;
-
- //排出
- case 17:
- counter.reset();
- tyokudo(arm_enc.getPulses(), 18);
- break;
-
- //1秒停止
- case 18:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 19;
- wheel_reset();
- }
- 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 21:
- counter.reset();
-
- if(right_limit == 3) {
- phase = 22;
- }
- 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 22:
- stop();
- counter.start();
- if(counter.read() > 0.5f) {
- phase = 23;
- wheel_reset();
- }
- break;
-
- //壁に当たるまで後進
- case 23:
- counter.reset();
-
- if(back_limit == 3) {
- phase = 24;
- }
- 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;
-
- //シーツ装填
- case 24:
- YELLOW_LED = 1;
- if(start_switch == 1) {
- wheel_reset();
- phase = 25;
- } else {
- stop();
- }
- break;
-
- //竿のラインまで前進
- case 25:
- counter.reset();
- front(21200);
- if((y_pulse1 > 21200) || (y_pulse2 > 21200)) {
- phase = 26;
- }
- break;
-
- //1秒停止
- case 26:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 27;
- wheel_reset();
- }
- break;
-
- //ちょっと左移動
- case 27:
- counter.reset();
- left(500);
- if((x_pulse1 > 500) || (x_pulse2 > 500)) {
- phase = 28;
- }
- break;
-
- //1秒停止
- case 28:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 29;
- wheel_reset();
- }
- 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 = 32;
- }
- 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 32:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 33;
- wheel_reset();
- }
- break;
-
- //掛けるところまで後進
- case 33:
- counter.reset();
- back(-9200);
- if((y_pulse1*-1 > 9200) || (y_pulse2*-1 > 9200)) {
- phase = 34;
- counter.start();
- }
- break;
-
- //1秒停止
- case 34:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 35;
- wheel_reset();
- }
- 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 = 37;
- wheel_reset();
- } else {
- right_arm_data[0] = 0xFF;
- left_arm_data[0] = 0x00;
- i2c.write(0x22, right_arm_data, 1);
- i2c.write(0x24, left_arm_data, 1);
- wait_us(20);
- }
- break;
-
- //アームアップ
- case 37:
- counter.reset();
- stop();
- arm_up(38);
- break;
-
- //カウンターリセット
- case 38:
- counter.reset();
- phase = 39;
- break;
-
- //シーツを掛ける
- case 39:
- counter.start();
- //fan_on(1.0f, 1.75f, FINAL_PHASE);
- fan_on(wind_time1, wind_time2, FINAL_PHASE);
- /*
- //1秒間ファン送風
- 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~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);
- }
- //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 = FINAL_PHASE;
- }
- */
- break;
-
- //終了っ！(守衛さん風)
- case FINAL_PHASE:
- default:
- //駆動系統OFF
- all_stop();
- break;
- }
+ blue_move();
}
-
//REDゾーン
else if(zone == RED) {
GREEN_LED = 0;
RED_LED = 1;
-
- switch(phase) {
-
- //スタート位置へセット
- case 0:
- //スタートスイッチが押されたか
- if(start_switch == 1) {
- wheel_reset();
- phase = 1;
- }
-
- //リミットが洗濯物台に触れているか
- if(right_limit == 3) {
- USR_LED1 = 1;
-
- } else {
- USR_LED1 = 0;
- }
- break;
+ red_move();
+ }
+ }
+}
- //回収アームを伸ばす
- case 1:
- counter.reset();
- kaisyu_nobasu(2);
- //サーボを開いておく
- servo_data[0] = 0x03;
- i2c.write(0x30, servo_data, 1);
- break;
-
- //1.0秒停止
- case 2:
- stop();
- servo_data[0] = 0x04;
- i2c.write(0x30, servo_data, 1);
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 3;
- wheel_reset();
- }
- break;
+void red_move(void) {
+ switch(phase) {
+
+ //スタート位置へセット
+ case 0:
+ //スタートスイッチが押されたか
+ if(start_switch == 1) {
+ wheel_reset();
+ phase = 1;
+ }
+
+ //リミットが洗濯物台に触れているか
+ if(right_limit == 3) {
+ USR_LED1 = 1;
+ } else {
+ USR_LED1 = 0;
+ }
+ break;
+
+ //回収アームを伸ばす
+ case 1:
+ kaisyu_nobasu(2);
+ //サーボを開いておく
+ servo_data[0] = 0x03;
+ i2c.write(0x30, servo_data, 1);
+ break;
+
+ //1.0秒停止
+ case 2:
+ if(stop_flag[0] == 0) {
+ stop();
+ servo_data[0] = 0x04;
+ i2c.write(0x30, servo_data, 1);
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[0] = 1;
+ }
+ if(counter.read() > 1.0f) {
+ phase = 3;
+ wheel_reset();
+ }
+ break;
+
+ //ちょっと前進
+ case 3:
+ front(800);
+ if((y_pulse1 > 800) || (y_pulse2 > 800)) {
+ phase = 4;
+ }
+ break;
- //ちょっと前進
- case 3:
- counter.reset();
- front(800);
- if((y_pulse1 > 800) || (y_pulse2 > 800)) {
- phase = 4;
- }
- break;
+ //0.5秒停止
+ case 4:
+ if(stop_flag[1] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[1] = 1;
+ }
+ if(counter.read() > 0.5f) {
+ phase = 5;
+ wheel_reset();
+ }
+ break;
- //0.5秒停止
- case 4:
- stop();
- counter.start();
- if(counter.read() > 0.5f) {
- phase = 5;
- wheel_reset();
- }
- break;
+ //回収アーム引っ込める
+ case 5:
+ USR_LED3 = 1;
+ kaisyu_hiku(6);
+ break;
- //回収アーム引っ込める
- case 5:
- USR_LED3 = 1;
- kaisyu_hiku(6);
- break;
+ //左移動
+ case 6:
+ USR_LED4 = 1;
+ left(11500);
+ if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
+ phase = 7;
+ }
+ break;
- //左移動
- case 6:
- USR_LED4 = 1;
- counter.reset();
- left(11500);
- if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
- phase = 7;
- }
- break;
+ //1秒停止
+ case 7:
+ if(stop_flag[2] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[2] = 1;
+ }
+ if(counter.read() > 1.0f) {
+ phase = 8;
+ wheel_reset();
+ }
+ break;
+
+ //右旋回(180°)
+ case 8:
+ turn_right(940);
+ if(sum_pulse > 940) {
+ phase = 9;
+ }
+ break;
- //1秒停止
- case 7:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 8;
- wheel_reset();
- }
- break;
+ //0.5秒停止
+ case 9:
+ if(stop_flag[3] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[3] = 1;
+ }
+ if(counter.read() > 0.5f) {
+ phase = 10;
+ wheel_reset();
+ }
+ break;
- //右旋回(180°)
- case 8:
- counter.reset();
- turn_right(940);
- if(sum_pulse > 940) {
- phase = 9;
- }
- break;
+ //壁に当たるまで前進
+ case 10:
+ if(front_limit == 3) {
+ phase = 11;
+ }
+ else if(front_limit != 3){
+ true_migimae_data[0] = 0xA0;
+ true_migiusiro_data[0] = 0xA0;
+ true_hidarimae_data[0] = 0xA0;
+ true_hidariusiro_data[0] = 0xA0;
+ 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 9:
- stop();
- counter.start();
- if(counter.read() > 0.5f) {
- phase = 10;
- wheel_reset();
- }
- break;
+ //0.5秒停止
+ case 11:
+ if(stop_flag[4] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[4] = 1;
+ }
+ if(counter.read() > 0.5f) {
+ phase = 12;
+ wheel_reset();
+ }
+ break;
- //壁に当たるまで前進
- case 10:
- counter.reset();
+ //壁に当たるまで右移動
+ case 12:
+ 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;
- if(front_limit == 3) {
- phase = 11;
- }
- 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;
+ //0.5秒停止
+ case 13:
+ if(stop_flag[5] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[5] = 1;
+ }
+ if(counter.read() > 0.5f) {
+ phase = 14;
+ wheel_reset();
+ }
+ break;
+
+ //排出
+ case 14:
+ tyokudo_nobasu(15);
+ break;
- //0.5秒停止
- case 11:
- stop();
- counter.start();
- if(counter.read() > 0.5f) {
- phase = 12;
- wheel_reset();
- }
- break;
+ //後進
+ case 15:
+ back(-5000);
+ if((y_pulse1*-1 > 5000) || (y_pulse2*-1 > 5000)) {
+ phase = 16;
+ }
+ break;
- //壁に当たるまで右移動
- case 12:
- counter.reset();
+ //0.5秒停止
+ case 16:
+ if(stop_flag[6] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[6] = 1;
+ }
+ if(counter.read() > 0.5f) {
+ phase = 17;
+ wheel_reset();
+ }
+ break;
- 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;
+ //排出しまう
+ case 17:
+ tyokudo_hiku(18);
+ break;
- //0.5秒停止
- case 13:
- stop();
- counter.start();
- if(counter.read() > 0.5f) {
- phase = 14;
- wheel_reset();
- }
- break;
+ //0.5秒停止
+ case 18:
+ if(stop_flag[7] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[7] = 1;
+ }
+ if(counter.read() > 0.5f) {
+ phase = 19;
+ wheel_reset();
+ }
+ break;
+
+ //壁に当たるまで右移動
+ case 19:
+ 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;
- //排出
- case 14:
- counter.reset();
- tyokudo(arm_enc.getPulses(), 15);
- break;
+ //0.5秒停止
+ case 20:
+ if(stop_flag[8] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[8] = 1;
+ }
+ if(counter.read() > 0.5f) {
+ phase = 21;
+ wheel_reset();
+ }
+ break;
- //1秒停止
- case 15:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 16;
- wheel_reset();
- }
- break;
+ //壁に当たるまで前進
+ case 21:
+ if(front_limit == 3) {
+ phase = 22;
+ }
+ else if(front_limit != 3){
+ true_migimae_data[0] = 0xA0;
+ true_migiusiro_data[0] = 0xA0;
+ true_hidarimae_data[0] = 0xA0;
+ true_hidariusiro_data[0] = 0xA0;
+ 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;
- //後進
- case 16:
- counter.reset();
- back(-5000);
- if((y_pulse1*-1 > 5000) || (y_pulse2*-1 > 5000)) {
- phase = 17;
- }
- break;
-
- //0.5秒停止
- case 17:
+ //シーツ装填
+ case 22:
+ YELLOW_LED = 1;
+ if(start_switch == 1) {
+ wheel_reset();
+ phase = 23;
+ } else {
+ if(stop_flag[9] == 0) {
stop();
- counter.start();
- if(counter.read() > 0.5f) {
- phase = 18;
- wheel_reset();
- }
- break;
-
- //壁に当たるまで右移動
- case 18:
- counter.reset();
-
- if(right_limit == 3) {
- phase = 19;
- }
- 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 19:
- stop();
- counter.start();
- if(counter.read() > 0.5f) {
- phase = 20;
- wheel_reset();
- }
- break;
-
- //壁に当たるまで前進
- case 20:
- counter.reset();
-
- if(front_limit == 3) {
- phase = 21;
- }
- 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;
-
- //シーツ装填
- case 21:
- YELLOW_LED = 1;
- if(start_switch == 1) {
- wheel_reset();
- phase = 22;
- } else {
- stop();
- }
- break;
-
- //竿のラインまで後進
- 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 25:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 26;
- wheel_reset();
- }
- break;
-
- //90°左旋回
- case 26:
- counter.reset();
- turn_left(500);
- if(sum_pulse > 500) {
- phase = 27;
- }
- break;
-
- //1秒停止
- case 27:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 28;
- wheel_reset();
- }
- break;
-
- //壁に当たるまで後進
- case 28:
- counter.reset();
-
- if(back_limit == 3) {
- phase = 29;
- }
- 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秒停止
- case 29:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 30;
- wheel_reset();
- }
- break;
-
- //掛けるところまで前進
- case 30:
- counter.reset();
- front(9500);
- if((y_pulse1 > 9500) || (y_pulse2 > 9500)) {
- phase = 31;
- counter.start();
- }
- break;
-
- //1秒停止
- case 31:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 32;
- wheel_reset();
- }
- break;
-
- //妨害防止の左旋回
- case 32:
- counter.reset();
- turn_left(30);
- if(sum_pulse > 30) {
- phase = 33;
- }
- break;
-
- //1秒停止
- case 33:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 34;
- wheel_reset();
- } else {
- right_arm_data[0] = 0xFF;
- left_arm_data[0] = 0x00;
- i2c.write(0x22, right_arm_data, 1);
- i2c.write(0x24, left_arm_data, 1);
- wait_us(20);
- }
- break;
-
- //カウンターリセット
- case 34:
+ counter.stop();
counter.reset();
counter.start();
- phase = 35;
- break;
+ stop_flag[9] = 1;
+ }
+ }
+ break;
+
+ //竿のラインまで後進
+ case 23:
+ back(-20500);
+ if((y_pulse1*-1 > 20500) || (y_pulse2*-1 > 20500)) {
+ phase = 24;
+ }
+ break;
+
+ //1秒停止
+ case 24:
+ if(stop_flag[10] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[10] = 1;
+ }
+ if(counter.read() > 1.0f) {
+ phase = 25;
+ wheel_reset();
+ }
+ break;
+
+ //ちょっと左移動
+ case 25:
+ left(400);
+ if((x_pulse1 > 400) || (x_pulse2 > 400)) {
+ phase = 26;
+ }
+ break;
+
+ //1秒停止
+ case 26:
+ if(stop_flag[11] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[11] = 1;
+ }
+ if(counter.read() > 1.0f) {
+ phase = 27;
+ wheel_reset();
+ }
+ break;
+
+ //90°左旋回
+ case 27:
+ turn_left(500);
+ if(sum_pulse > 500) {
+ phase = 28;
+ }
+ break;
+
+ //1秒停止
+ case 28:
+ if(stop_flag[12] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[12] = 1;
+ }
+ if(counter.read() > 1.0f) {
+ phase = 29;
+ wheel_reset();
+ }
+ break;
- //アームアップ
- case 35:
- counter.reset();
- stop();
- arm_up(36);
- break;
+ //壁に当たるまで後進
+ case 29:
+ if(back_limit == 3) {
+ phase = 30;
+ }
+ else if(back_limit != 3){
+ true_migimae_data[0] = 0x60;
+ true_migiusiro_data[0] = 0x60;
+ true_hidarimae_data[0] = 0x60;
+ true_hidariusiro_data[0] = 0x60;
+ 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 30:
+ if(stop_flag[13] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[13] = 1;
+ }
+ if(counter.read() > 1.0f) {
+ phase = 31;
+ wheel_reset();
+ }
+ break;
+
+ //掛けるところまで前進
+ case 31:
+ front(9200);
+ if((y_pulse1 > 9200) || (y_pulse2 > 9200)) {
+ phase = 32;
+ counter.start();
+ }
+ break;
+
+ //1秒停止
+ case 32:
+ if(stop_flag[14] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[14] = 1;
+ }
+ if(counter.read() > 1.0f) {
+ phase = 33;
+ wheel_reset();
+ }
+ break;
+
+ //妨害防止の左旋回
+ case 33:
+ turn_left(20);
+ if(sum_pulse > 20) {
+ phase = 34;
+ }
+ break;
+
+ //1秒停止
+ case 34:
+ if(stop_flag[15] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[15] = 1;
+ }
+ if(counter.read() > 1.0f) {
+ phase = 35;
+ wheel_reset();
+ }
+ break;
+
+ //アームアップ
+ case 35:
+ arm_up(36);
+ if(stop_flag[16] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[16] = 1;
+ }
+ if(counter.read() > 2.0f) {
+ fan_data[0] = 0xFF;
+ } else {
+ fan_data[0] = 0x80;
+ }
+ i2c.write(0x26, fan_data, 1);
+ i2c.write(0x28, fan_data, 1);
+ wait_us(20);
+ break;
- //カウンターリセット
- case 36:
- counter.reset();
- phase = 37;
- break;
+ //シーツを掛ける
+ case 36:
+ if(stop_flag[17] == 0) {
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[17] = 1;
+ }
+ fan_on(wind_time1, wind_time2, FINAL_PHASE);
+ break;
+
+ //終了っ！(守衛さん風)
+ case FINAL_PHASE:
+ default:
+ //駆動系統OFF
+ all_stop();
+ break;
+ }
+}
+
+void blue_move(void) {
+ switch(phase) {
+
+ //スタート位置へセット
+ case 0:
+ //スタートスイッチが押されたか
+ if(start_switch == 1) {
+ wheel_reset();
+ phase = 1;
+ }
+
+ //リミットが洗濯物台に触れているか
+ if(right_limit == 3) {
+ USR_LED1 = 1;
+ } else {
+ USR_LED1 = 0;
+ }
+ break;
+
+ //回収アームを伸ばす
+ case 1:
+ kaisyu_nobasu(2);
+ //サーボを開いておく
+ servo_data[0] = 0x03;
+ i2c.write(0x30, servo_data, 1);
+ break;
+
+ //1.0秒停止
+ case 2:
+ if(stop_flag[0] == 0) {
+ stop();
+ servo_data[0] = 0x04;
+ i2c.write(0x30, servo_data, 1);
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[0] = 1;
+ }
+ if(counter.read() > 1.0f) {
+ phase = 3;
+ wheel_reset();
+ }
+ break;
+
+ //壁に当たるまで前進
+ case 3:
+ if(front_limit == 3) {
+ phase = 4;
+ }
+ else if(front_limit != 3){
+ true_migimae_data[0] = 0xA0;
+ true_migiusiro_data[0] = 0xA0;
+ true_hidarimae_data[0] = 0xA0;
+ true_hidariusiro_data[0] = 0xA0;
+ 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);
+ }
+ /*
+ front(800);
+ if((y_pulse1 > 800) || (y_pulse2 > 800)) {
+ phase = 4;
+ }
+ */
+ break;
- //シーツを掛ける
- case 37:
- counter.start();
- //fan_on(1.0f, 1.75f, FINAL_PHASE);
- fan_on(wind_time1, wind_time2, FINAL_PHASE);
+ //0.5秒停止
+ case 4:
+ if(stop_flag[1] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[1] = 1;
+ }
+ if(counter.read() > 0.5f) {
+ phase = 5;
+ wheel_reset();
+ }
+ break;
+
+ //回収アーム引っ込める
+ case 5:
+ USR_LED3 = 1;
+ kaisyu_hiku(6);
+ break;
+
+ //0.5秒停止
+ case 6:
+ if(stop_flag[2] == 0) {
+ USR_LED4 = 1;
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[2] = 1;
+ }
+ if(counter.read() > 0.5f) {
+ phase = 7;
+ wheel_reset();
+ }
+ break;
+
+ //ちょっと後進
+ case 7:
+ back(-700);
+ if((y_pulse1*-1 > 700) || (y_pulse2*-1 > 700)) {
+ phase = 8;
+ }
+ break;
+
+ //0.5秒停止
+ case 8:
+ if(stop_flag[3] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[3] = 1;
+ }
+ if(counter.read() > 0.5f) {
+ phase = 9;
+ wheel_reset();
+ }
+ break;
+
+ //左移動
+ case 9:
+ left(11500);
+ if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
+ phase = 10;
+ }
+ break;
+
+ //1秒停止
+ case 10:
+ if(stop_flag[4] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[4] = 1;
+ }
+ if(counter.read() > 1.0f) {
+ phase = 11;
+ wheel_reset();
+ }
+ break;
+
+ //右旋回(180°)
+ case 11:
+ counter.reset();
+ turn_right(950);
+ if(sum_pulse > 950) {
+ phase = 12;
+ }
+ break;
+
+ //0.5秒停止
+ case 12:
+ if(stop_flag[5] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[5] = 1;
+ }
+ if(counter.read() > 0.5f) {
+ phase = 13;
+ wheel_reset();
+ }
+ break;
- /*
- //1秒間ファン送風
- 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~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);
- }
- //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 = FINAL_PHASE;
- }
- */
- break;
+ //壁に当たるまで後進
+ case 13:
+ if(back_limit == 3) {
+ phase = 14;
+ }
+ else if(back_limit != 3){
+ true_migimae_data[0] = 0x60;
+ true_migiusiro_data[0] = 0x60;
+ true_hidarimae_data[0] = 0x60;
+ true_hidariusiro_data[0] = 0x60;
+ 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 14:
+ if(stop_flag[6] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[6] = 1;
+ }
+ if(counter.read() > 0.5f) {
+ phase = 15;
+ wheel_reset();
+ }
+ break;
+
+ //壁に当たるまで右移動
+ case 15:
+ if(right_limit == 3) {
+ phase = 16;
+ }
+ 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 16:
+ if(stop_flag[7] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[7] = 1;
+ }
+ if(counter.read() > 0.5f) {
+ phase = 17;
+ wheel_reset();
+ }
+ break;
+
+ //排出
+ case 17:
+ tyokudo_nobasu(18);
+ break;
+
+ //前進
+ case 18:
+ front(5000);
+ if((y_pulse1 > 5000) || (y_pulse2 > 5000)) {
+ phase = 19;
+ }
+ break;
+
+ //0.5秒停止
+ case 19:
+ if(stop_flag[8] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[8] = 1;
+ }
+ if(counter.read() > 0.5f) {
+ phase = 20;
+ wheel_reset();
+ }
+ break;
+
+ //排出しまう
+ case 20:
+ tyokudo_hiku(21);
+ break;
+
+ //0.5秒停止
+ case 21:
+ if(stop_flag[9] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[9] = 1;
+ }
+ if(counter.read() > 0.5f) {
+ phase = 22;
+ wheel_reset();
+ }
+ break;
+
+ //壁に当たるまで右移動
+ case 22:
+ if(right_limit == 3) {
+ phase = 23;
+ }
+ 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 23:
+ if(stop_flag[10] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[10] = 1;
+ }
+ if(counter.read() > 0.5f) {
+ phase = 24;
+ wheel_reset();
+ }
+ break;
+
+ //壁に当たるまで後進
+ case 24:
+ if(back_limit == 3) {
+ phase = 25;
+ }
+ else if(back_limit != 3){
+ true_migimae_data[0] = 0x60;
+ true_migiusiro_data[0] = 0x60;
+ true_hidarimae_data[0] = 0x60;
+ true_hidariusiro_data[0] = 0x60;
+ 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;
+
+ //シーツ装填
+ case 25:
+ YELLOW_LED = 1;
+ if(start_switch == 1) {
+ wheel_reset();
+ phase = 26;
+ } else {
+ if(stop_flag[11] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[11] = 1;
+ }
+ }
+ break;
- //終了っ！(守衛さん風)
- case FINAL_PHASE:
- default:
- //駆動系統OFF
- all_stop();
- break;
+ //竿のラインまで前進
+ case 26:
+ front(21200);
+ if((y_pulse1 > 21200) || (y_pulse2 > 21200)) {
+ phase = 27;
+ }
+ break;
+
+ //1秒停止
+ case 27:
+ if(stop_flag[12] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[12] = 1;
+ }
+ if(counter.read() > 1.0f) {
+ phase = 28;
+ wheel_reset();
+ }
+ break;
+
+ //ちょっと左移動
+ case 28:
+ left(400);
+ if((x_pulse1 > 400) || (x_pulse2 > 400)) {
+ phase = 29;
+ }
+ break;
+
+ //1秒停止
+ case 29:
+ if(stop_flag[13] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[13] = 1;
+ }
+ if(counter.read() > 1.0f) {
+ phase = 30;
+ wheel_reset();
+ }
+ break;
+
+ //90°右旋回
+ case 30:
+ turn_right(480);
+ if(sum_pulse > 480) {
+ phase = 31;
+ }
+ break;
+
+ //1秒停止
+ case 31:
+ if(stop_flag[14] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[14] = 1;
+ }
+ if(counter.read() > 1.0f) {
+ phase = 32;
+ wheel_reset();
+ }
+ break;
+
+ //壁に当たるまで前進
+ case 32:
+ if(front_limit == 3) {
+ phase = 33;
+ }
+ else if(front_limit != 3){
+ true_migimae_data[0] = 0xA0;
+ true_migiusiro_data[0] = 0xA0;
+ true_hidarimae_data[0] = 0xA0;
+ true_hidariusiro_data[0] = 0xA0;
+ 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 33:
+ if(stop_flag[15] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[15] = 1;
+ }
+ if(counter.read() > 1.0f) {
+ phase = 34;
+ wheel_reset();
+ }
+ break;
+
+ //掛けるところまで後進
+ case 34:
+ back(-9200);
+ if((y_pulse1*-1 > 9200) || (y_pulse2*-1 > 9200)) {
+ phase = 35;
+ counter.start();
+ }
+ break;
+
+ //1秒停止
+ case 35:
+ if(stop_flag[16] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[16] = 1;
+ }
+ if(counter.read() > 1.0f) {
+ phase = 36;
+ wheel_reset();
+ }
+ break;
+
+ //妨害防止の右旋回
+ case 36:
+ turn_right(20);
+ if(sum_pulse > 20) {
+ phase = 37;
+ }
+ break;
+
+ //1秒停止
+ case 37:
+ if(stop_flag[17] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[17] = 1;
+ }
+ if(counter.read() > 1.0f) {
+ phase = 38;
+ wheel_reset();
+ }
+ break;
+
+ //アームアップ
+ case 38:
+ arm_up(39);
+ if(stop_flag[18] == 0) {
+ stop();
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[18] = 1;
+ }
+ if(counter.read() > 2.0f) {
+ fan_data[0] = 0xFF;
+ } else {
+ fan_data[0] = 0x80;
+ }
+ i2c.write(0x26, fan_data, 1);
+ i2c.write(0x28, fan_data, 1);
+ wait_us(20);
+ break;
+
+ //シーツを掛ける
+ case 39:
+ if(stop_flag[19] == 0) {
+ counter.stop();
+ counter.reset();
+ counter.start();
+ stop_flag[19] = 1;
+ }
+ fan_on(wind_time1, wind_time2, FINAL_PHASE);
+ break;
+
+ //終了っ！(守衛さん風)
+ case FINAL_PHASE:
+ default:
+ //駆動系統OFF
+ all_stop();
+ break;
}
}
@@ -1198,7 +1312,7 @@
YELLOW_LED = 0;
USR_LED3 = 0; USR_LED4 = 0;
-
+
//非常停止関連
pic.baud(19200);
pic.format(8, Serial::None, 1);
@@ -1581,47 +1695,6 @@
void kaisyu_nobasu(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;
- kaisyu_phase = 2;
- phase = next_phase;
- }
- //後ろのリミットが押されたら強制停止
- if(kaisyu_usiro_limit == 1) {
- arm_motor[0] = 0x80;
- kaisyu_phase = 2;
- phase = next_phase;
- }
- break;
-
- default:
- arm_motor[0] = 0x80;
- break;
- }
- */
-
//前進->減速
//3000pulseまで高速前進
if(arm_pulse < 3000) {
@@ -1652,42 +1725,6 @@
void kaisyu_hiku(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;
- }
- */
-
//後進->減速
//500pulseより大きい範囲で高速後進
if(arm_pulse > 500) {
@@ -1790,11 +1827,127 @@
wait_us(20);
}
+void tyokudo_nobasu(int next_phase) {
+
+ if(tyokudo_mae_limit == 0) {
+
+ drop_motor[0] = 0xFF;
+
+ //2000pulseまで高速前進
+ if(arm_pulse < 2000) {
+ arm_motor[0] = 0xFF;
+ }
+ //2000pulse以上で低速前進
+ else if(arm_pulse >= 2000) {
+ arm_motor[0] = 0xB3;
+ }
+ //パルスが2500を終えたらアームのみ強制停止
+ else if(arm_pulse > 2500) {
+ arm_motor[0] = 0x80;
+ } else {
+ arm_motor[0] = 0x80;
+ }
+
+ //後ろのリミットが押されたら強制停止
+ if(kaisyu_usiro_limit == 1) {
+ arm_motor[0] = 0x80;
+ }
+ }
+ else if(tyokudo_mae_limit == 1) {
+
+ drop_motor[0] = 0x80;
+
+ //2000pulseまで高速前進
+ if(arm_pulse < 2000) {
+ arm_motor[0] = 0xFF;
+ }
+ //2000pulse以上で低速前進
+ else if(arm_pulse >= 2000) {
+ arm_motor[0] = 0xB3;
+ }
+ //パルスが2500を終えたらアームのみ強制停止
+ else if(arm_pulse > 2500) {
+ arm_motor[0] = 0x80;
+ phase = next_phase;
+ } else {
+ arm_motor[0] = 0x80;
+ }
+
+ //後ろのリミットが押されたら強制停止
+ if(kaisyu_usiro_limit == 1) {
+ arm_motor[0] = 0x80;
+ phase = next_phase;
+ }
+ }
+ //回収MD・排出MDへ書き込み
+ i2c.write(0x18, arm_motor, 1);
+ i2c.write(0x20, drop_motor, 1);
+ wait_us(20);
+}
+
+void tyokudo_hiku(int next_phase) {
+
+ if(tyokudo_usiro_limit == 0) {
+
+ drop_motor[0] = 0x00;
+
+ //500pulseより大きい範囲で高速後進
+ if(arm_pulse > 500) {
+ arm_motor[0] = 0x00;
+ }
+ //500pulse以下になったら低速後進
+ else if(arm_pulse <= 500) {
+ arm_motor[0] = 0x4C;
+ }
+ //0pulse以下で停止
+ else if(arm_pulse <= 0) {
+ arm_motor[0] = 0x80;
+ } else {
+ arm_motor[0] = 0x80;
+ }
+
+ //後ろのリミットが押されたら強制停止
+ if(kaisyu_mae_limit == 1) {
+ arm_motor[0] = 0x80;
+ }
+ }
+ else if(tyokudo_usiro_limit == 1) {
+
+ drop_motor[0] = 0x80;
+
+ //500pulseより大きい範囲で高速後進
+ if(arm_pulse > 500) {
+ arm_motor[0] = 0x00;
+ }
+ //500pulse以下になったら低速後進
+ else if(arm_pulse <= 500) {
+ arm_motor[0] = 0x4C;
+ }
+ //0pulse以下で停止
+ else if(arm_pulse <= 0) {
+ arm_motor[0] = 0x80;
+ phase = next_phase;
+ } else {
+ arm_motor[0] = 0x80;
+ }
+
+ //後ろのリミットが押されたら強制停止
+ if(kaisyu_mae_limit == 1) {
+ arm_motor[0] = 0x80;
+ phase = next_phase;
+ }
+ }
+ //回収MD・排出MDへ書き込み
+ i2c.write(0x18, arm_motor, 1);
+ i2c.write(0x20, drop_motor, 1);
+ wait_us(20);
+}
+
void arm_up(int next_phase) {
//両腕、上限リミットが押されてなかったら上昇
if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 0)) {
- right_arm_data[0] = 0xFF; left_arm_data[0] = 0x00;
+ right_arm_data[0] = 0x00; left_arm_data[0] = 0x00;
}
//右腕のみリミットが押されたら左腕のみ上昇
else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 0)) {
@@ -1802,7 +1955,7 @@
}
//左腕のみリミットが押されたら右腕のみ上昇
else if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 1)) {
- right_arm_data[0] = 0xFF; left_arm_data[0] = 0x80;
+ right_arm_data[0] = 0x00; left_arm_data[0] = 0x80;
}
//両腕、上限リミットが押されたら停止
else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 1)) {