2019NHK_teamA
2019年10月02日AM11:14現在のもの(青ゾーンは変更なし)
Diff: main.cpp
- Revision:
- 24:d12bc20c01c2
- Parent:
- 23:1e4d7540715f
- Child:
- 25:ce789ea15628
--- 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)) {