2019NHK_teamA
/
2019_A_ver7-4
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 24:d12bc20c01c2, committed 2019-09-29
- Comitter:
- yuron
- Date:
- Sun Sep 29 17:01:17 2019 +0000
- Parent:
- 23:1e4d7540715f
- Commit message:
- aaaaa
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 1e4d7540715f -r d12bc20c01c2 main.cpp
--- a/main.cpp Sat Sep 28 01:24:03 2019 +0000
+++ b/main.cpp Sun Sep 29 17:01:17 2019 +0000
@@ -5,7 +5,7 @@
/* Actuator: RS-555*4, RS-380*2, RZ-735*2, RS-385*2, PWM_Servo(KONDO)*2 */
/* Sensor: encorder*4, limit_switch*14 */
/* ------------------------------------------------------------------- */
-/* added red zone(checked) */
+/* Both of areas are compleated! */
/* ------------------------------------------------------------------- */
#include "mbed.h"
#include "math.h"
@@ -79,6 +79,7 @@
DigitalOut USR_LED4(PC_3);
DigitalOut GREEN_LED(D8);
DigitalOut RED_LED(D10);
+DigitalOut YELLOW_LED(D9);
//遠隔非常停止ユニットLED
AnalogOut myled(A2);
@@ -150,17 +151,17 @@
//吐き出し機構の下限
bool tyokudo_usiro_limit = 0;
-int masked_lower_front_limit_data = 0xFF;
-int masked_lower_back_limit_data = 0xFF;
-int masked_lower_right_limit_data = 0xFF;
-int masked_kaisyu_mae_limit_data = 0xFF;
-int masked_kaisyu_usiro_limit_data = 0xFF;
-int masked_right_arm_lower_limit_data = 0xFF;
-int masked_right_arm_upper_limit_data = 0xFF;
-int masked_left_arm_lower_limit_data = 0xFF;
-int masked_left_arm_upper_limit_data = 0xFF;
-int masked_tyokudo_mae_limit_data = 0xFF;
-int masked_tyokudo_usiro_limit_data = 0xFF;
+int masked_lower_front_limit_data = 0b11111111;
+int masked_lower_back_limit_data = 0b11111111;
+int masked_lower_right_limit_data = 0b11111111;
+int masked_kaisyu_mae_limit_data = 0b11111111;
+int masked_kaisyu_usiro_limit_data = 0b11111111;
+int masked_right_arm_lower_limit_data = 0b11111111;
+int masked_right_arm_upper_limit_data = 0b11111111;
+int masked_left_arm_lower_limit_data = 0b11111111;
+int masked_left_arm_upper_limit_data = 0b11111111;
+int masked_tyokudo_mae_limit_data = 0b11111111;
+int masked_tyokudo_usiro_limit_data = 0b11111111;
//関数のプロトタイプ宣言
void init(void);
@@ -229,7 +230,7 @@
/*
if(start_switch == 1) {
- phase = 39;
+ phase = 15;
}
*/
@@ -315,8 +316,8 @@
//左移動
case 7:
counter.reset();
- left(11500);
- if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
+ left(10500);
+ if((x_pulse1 > 10500) || (x_pulse2 > 10500)) {
phase = 8;
}
break;
@@ -497,6 +498,7 @@
//シーツ装填
case 22:
+ YELLOW_LED = 1;
if(start_switch == 1) {
wheel_reset();
phase = 23;
@@ -736,7 +738,7 @@
servo_data[0] = 0x04;
i2c.write(0x30, servo_data, 1);
counter.start();
- if(counter.read() > 1.0f) {
+ if(counter.read() > 0.5f) {
phase = 3;
wheel_reset();
}
@@ -747,7 +749,8 @@
counter.reset();
front(800);
if((y_pulse1 > 800) || (y_pulse2 > 800)) {
- phase = 4;
+ //phase = 4;
+ phase = 5;
}
break;
@@ -755,7 +758,7 @@
case 4:
stop();
counter.start();
- if(counter.read() > 1.0f) {
+ if(counter.read() > 0.5f) {
phase = 5;
wheel_reset();
}
@@ -764,18 +767,9 @@
//回収アーム引っ込める
case 5:
counter.reset();
- kaisyu_hiku(arm_pulse, 6);
- break;
-
- //1秒停止
- case 6:
- stop();
- counter.start();
- if(counter.read() > 1.0f) {
- phase = 7;
- wheel_reset();
- }
- break;
+ //kaisyu_hiku(arm_pulse, 6);
+ kaisyu_hiku(arm_pulse, 7);
+ break;
//左移動
case 7:
@@ -799,8 +793,9 @@
//右旋回(180°)
case 9:
counter.reset();
- turn_right(975);
- if(sum_pulse > 975) {
+ //turn_right(975);
+ turn_right(960);
+ if(sum_pulse > 960) {
phase = 10;
}
break;
@@ -809,7 +804,7 @@
case 10:
stop();
counter.start();
- if(counter.read() > 1.0f) {
+ if(counter.read() > 0.5f) {
phase = 11;
wheel_reset();
}
@@ -839,7 +834,7 @@
case 12:
stop();
counter.start();
- if(counter.read() > 1.0f) {
+ if(counter.read() > 0.5f) {
phase = 13;
wheel_reset();
}
@@ -869,7 +864,7 @@
case 14:
stop();
counter.start();
- if(counter.read() > 1.0f) {
+ if(counter.read() > 0.5f) {
phase = 15;
wheel_reset();
}
@@ -878,7 +873,8 @@
//排出
case 15:
counter.reset();
- tyokudo(arm_enc.getPulses(), 16);
+ //tyokudo(arm_enc.getPulses(), 16);
+ tyokudo(arm_enc.getPulses(), 17);
break;
//1秒停止
@@ -904,7 +900,7 @@
case 18:
stop();
counter.start();
- if(counter.read() > 1.0f) {
+ if(counter.read() > 0.5f) {
phase = 19;
wheel_reset();
}
@@ -934,7 +930,7 @@
case 20:
stop();
counter.start();
- if(counter.read() > 1.0f) {
+ if(counter.read() > 0.5f) {
phase = 21;
wheel_reset();
}
@@ -962,6 +958,7 @@
//シーツ装填
case 22:
+ YELLOW_LED = 1;
if(start_switch == 1) {
wheel_reset();
phase = 23;
@@ -1011,9 +1008,8 @@
//90°左旋回
case 27:
counter.reset();
- //turn_left(465);
- turn_left(485);
- if(sum_pulse > 485) {
+ turn_left(500);
+ if(sum_pulse > 500) {
phase = 28;
}
break;
@@ -1061,8 +1057,8 @@
//掛けるところまで前進
case 31:
counter.reset();
- front(10000);
- if((y_pulse1 > 10000) || (y_pulse2 > 10000)) {
+ front(11000);
+ if((y_pulse1 > 11000) || (y_pulse2 > 11000)) {
phase = 32;
counter.start();
}
@@ -1130,7 +1126,7 @@
counter.start();
//1秒間ファン送風
- if(counter.read() <= 2.0f) {
+ if(counter.read() <= 1.0f) {
fan_data[0] = 0xFF;
i2c.write(0x26, fan_data, 1);
i2c.write(0x28, fan_data, 1);
@@ -1138,7 +1134,7 @@
i2c.write(0x30, servo_data, 1);
}
//1~3秒の間でサーボを開放
- else if((counter.read() > 2.0f) && (counter.read() <= 4.0f)) {
+ else if((counter.read() > 1.0f) && (counter.read() <= 3.0f)) {
fan_data[0] = 0xFF;
i2c.write(0x26, fan_data, 1);
i2c.write(0x28, fan_data, 1);
@@ -1146,7 +1142,7 @@
i2c.write(0x30, servo_data, 1);
}
//3秒過ぎたら終わり
- else if(counter.read() > 4.0f) {
+ else if(counter.read() > 3.0f) {
fan_data[0] = 0x80;
i2c.write(0x26, fan_data, 1);
i2c.write(0x28, fan_data, 1);
@@ -1176,6 +1172,8 @@
start_switch.mode(PullUp);
zone_switch.mode(PullDown);
+
+ YELLOW_LED = 0;
//非常停止関連
pic.baud(19200);
@@ -1228,6 +1226,7 @@
}
void print_pulses(void) {
+ //pc.printf("%d\n\r", RDATA);
//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);
@@ -1246,7 +1245,7 @@
else if(RDATA == '9'){
myled = 0.2;
emergency = 0;
-
+ /*
//終了phaseで駆動系統OFF
if(phase >= 39) {
emergency = 1;
@@ -1254,6 +1253,7 @@
else if(phase < 39) {
emergency = 0;
}
+ */
}
}
@@ -1584,8 +1584,8 @@
//後ろのリミットが押されたら強制停止
if(kaisyu_usiro_limit == 1) {
arm_motor[0] = 0x80;
+ kaisyu_phase = 2;
phase = next_phase;
- kaisyu_phase = 2;
}
break;
@@ -1671,20 +1671,31 @@
//直動の前リミットが押されたら
else if(tyokudo_mae_limit == 1) {
//高速後進
- arm_motor[0] = 0x40;
+ arm_motor[0] = 0x4C;
drop_motor[0] = 0x00;
tyokudo_phase = 1;
}
break;
case 1:
- //後進->減速
- //リミットが押されたら強制停止
+ //後進->停止
if(tyokudo_usiro_limit == 1) {
+ drop_motor[0] = 0x80;
+
+ if(kaisyu_mae_limit == 1) {
+ arm_motor[0] = 0x80;
+ tyokudo_phase = 2;
+ phase = next_phase;
+ }
+ }
+ if(kaisyu_mae_limit == 1) {
arm_motor[0] = 0x80;
- drop_motor[0] = 0x80;
- tyokudo_phase = 2;
- phase = next_phase;
+
+ if(tyokudo_usiro_limit == 1) {
+ drop_motor[0] = 0x80;
+ tyokudo_phase = 2;
+ phase = next_phase;
+ }
}
break;
@@ -1693,7 +1704,7 @@
drop_motor[0] = 0x80;
break;
}
-
+ //回収MD・排出MDへ書き込み
i2c.write(0x18, arm_motor, 1);
i2c.write(0x20, drop_motor, 1);
}
@@ -2035,11 +2046,10 @@
//制御量の最小、最大
//左進(目標まで達していない)
if((x_pulse1 < target) && (x_pulse2 < target)) {
- 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に近いほうが速い
+ left_migimae.setOutputLimits(0xEC, 0xED);
+ left_migiusiro.setOutputLimits(0x7A, 0x7B);
+ left_hidarimae.setOutputLimits(0x6E, 0x6F);
+ left_hidariusiro.setOutputLimits(0xFE, 0xFF);
}
//停止(目標より行き過ぎ)
else if((x_pulse1 > target) && (x_pulse2 > target)) {