2019NHK_teamA
/
2019_A_ver7-6
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 26:81346a21d301, committed 2019-10-08
- Comitter:
- yuron
- Date:
- Tue Oct 08 02:03:01 2019 +0000
- Parent:
- 25:ce789ea15628
- Commit message:
- aaaaaaaaaa;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Oct 02 02:13:36 2019 +0000
+++ b/main.cpp Tue Oct 08 02:03:01 2019 +0000
@@ -12,9 +12,6 @@
#include "QEI.h"
#include "PID.h"
-//直進補正の為の前後・左右の回転差の許容値
-#define wheel_difference 100
-
//終了phase
#define FINAL_PHASE 50
@@ -196,6 +193,8 @@
init();
init_send();
+ //後で消せ俺さん！！
+ //phase = 50;
//起動時にゾーンを読んでからループに入る(試合中誤ってスイッチ押すのを防止)
while(1) {
@@ -223,6 +222,12 @@
i2c.write(0x30, servo_data, 1);
}
+ /*
+ if(start_switch == 1) {
+ phase = 37;
+ }
+ */
+
//青ゾーン
if(zone == BLUE) {
GREEN_LED = 1;
@@ -255,13 +260,13 @@
i2c.write(0x30, servo_data, 1);
break;
- //0.5秒停止
+ //1秒停止
case 2:
stop();
servo_data[0] = 0x04;
i2c.write(0x30, servo_data, 1);
counter.start();
- if(counter.read() > 0.5f) {
+ if(counter.read() > 1.0f) {
phase = 3;
wheel_reset();
}
@@ -271,7 +276,7 @@
case 3:
counter.reset();
front(800);
- if((y_pulse1 > 800) || (y_pulse2 > 800)) {
+ if((y_pulse1 > 700) || (y_pulse2 > 700)) {
phase = 4;
}
if(front_limit == 3) {
@@ -291,8 +296,17 @@
//回収アーム引っ込める
case 5:
- counter.reset();
- kaisyu_hiku(arm_pulse, 6);
+ USR_LED3 = 1;
+ //kaisyu_hiku(arm_pulse, 6);
+
+ if(kaisyu_mae_limit == 0) {
+ arm_motor[0] = 0x4C;
+ }
+ else if(kaisyu_mae_limit == 1) {
+ arm_motor[0] = 0x80;
+ phase = 6;
+ }
+
break;
//0.5秒停止
@@ -638,71 +652,32 @@
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 38;
+ phase = 37;
wheel_reset();
} else {
right_arm_data[0] = 0xFF;
- left_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();
- counter.start();
- arm_up(39);
- phase = 38;
- break;
- */
-
- //アームアップ
- case 38:
- counter.reset();
stop();
- arm_up(39);
- //1秒間リミットを読まずに無条件で上昇(チャタリングによる誤作動防止)
- /*
- 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(39);
- }
- */
+ arm_up(38);
break;
//カウンターリセット
- case 39:
+ case 38:
counter.reset();
- //phase = 40;
- phase = 42;
+ phase = 39;
break;
- //1秒停止
- case 40:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 41;
- wheel_reset();
- }
- break;
-
- //カウンターリセット
- case 41:
- counter.reset();
- phase = 42;
- break;
-
//シーツを掛ける
- case 42:
+ case 39:
counter.start();
//1秒間ファン送風
@@ -774,13 +749,13 @@
i2c.write(0x30, servo_data, 1);
break;
- //0.5秒停止
+ //1.0秒停止
case 2:
stop();
servo_data[0] = 0x04;
i2c.write(0x30, servo_data, 1);
counter.start();
- if(counter.read() > 0.5f) {
+ if(counter.read() > 1.0f) {
phase = 3;
wheel_reset();
}
@@ -807,8 +782,17 @@
//回収アーム引っ込める
case 5:
- counter.reset();
+ USR_LED3 = 1;
kaisyu_hiku(arm_pulse, 6);
+ /*
+ if(kaisyu_mae_limit == 0) {
+ arm_motor[0] = 0x4C;
+ }
+ else if(kaisyu_mae_limit == 1) {
+ arm_motor[0] = 0x80;
+ phase = 6;
+ }
+ */
break;
//左移動
@@ -833,9 +817,8 @@
//右旋回(180°)
case 8:
counter.reset();
- //turn_right(975);
- turn_right(960);
- if(sum_pulse > 960) {
+ turn_right(940);
+ if(sum_pulse > 940) {
phase = 9;
}
break;
@@ -1096,8 +1079,8 @@
//掛けるところまで前進
case 30:
counter.reset();
- front(10600);
- if((y_pulse1 > 10600) || (y_pulse2 > 10600)) {
+ front(9500);
+ if((y_pulse1 > 9500) || (y_pulse2 > 9500)) {
phase = 31;
counter.start();
}
@@ -1129,6 +1112,12 @@
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;
@@ -1141,17 +1130,9 @@
//アームアップ
case 35:
+ counter.reset();
stop();
- //1秒間リミットを読まずに無条件で上昇(チャタリングによる誤作動防止)
- 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(36);
- }
+ arm_up(36);
break;
//カウンターリセット
@@ -1159,26 +1140,9 @@
counter.reset();
phase = 37;
break;
-
- //1秒停止
- case 37:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 38;
- wheel_reset();
- }
- break;
-
- //カウンターリセット
- case 38:
- counter.reset();
- counter.start();
- phase = 39;
- break;
//シーツを掛ける
- case 39:
+ case 37:
counter.start();
//1秒間ファン送風
@@ -1230,6 +1194,7 @@
zone_switch.mode(PullDown);
YELLOW_LED = 0;
+ USR_LED3 = 0; USR_LED4 = 0;
//非常停止関連
pic.baud(19200);
@@ -1282,7 +1247,7 @@
}
void print_pulses(void) {
- pc.printf("%d\n\r", arm_pulse);
+ //pc.printf("p: %d, kp: %d, pulse: %d\n\r", phase, kaisyu_phase, arm_pulse);
//pc.printf("p: %d, k_p: %d, pulse: %d\n\r", phase, kaisyu_phase, arm_pulse);
//pc.printf("X1: %d, X2: %d, Y1: %d, Y2: %d, sum: %d\n\r", abs(x_pulse1), x_pulse2, abs(y_pulse1), y_pulse2, sum_pulse);
//pc.printf("%d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", front_limit, back_limit, right_limit, kaisyu_mae_limit, kaisyu_usiro_limit,
@@ -1769,11 +1734,11 @@
//両腕、上限リミットが押されてなかったら上昇
if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 0)) {
- right_arm_data[0] = 0xFF; left_arm_data[0] = 0xFF;
+ right_arm_data[0] = 0xFF; left_arm_data[0] = 0x00;
}
//右腕のみリミットが押されたら左腕のみ上昇
else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 0)) {
- right_arm_data[0] = 0x80; left_arm_data[0] = 0xFF;
+ right_arm_data[0] = 0x80; left_arm_data[0] = 0x00;
}
//左腕のみリミットが押されたら右腕のみ上昇
else if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 1)) {