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.
Revision 28:e95db716197d, committed 2019-10-23
- Comitter:
- yuron
- Date:
- Wed Oct 23 13:52:52 2019 +0000
- Parent:
- 27:4f2fc7172b31
- Commit message:
- aaa;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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)) {