2019NHK_teamA
/
2019_A_ver7-7
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 27:4f2fc7172b31, committed 2019-10-19
- Comitter:
- yuron
- Date:
- Sat Oct 19 01:10:39 2019 +0000
- Parent:
- 26:81346a21d301
- Commit message:
- aaa;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Oct 08 02:03:01 2019 +0000
+++ b/main.cpp Sat Oct 19 01:10:39 2019 +0000
@@ -2,7 +2,7 @@
/* NHK ROBOCON 2019 Ibaraki Kosen A team Automatic */
/* Nucleo Type: F446RE */
/* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com */
-/* Actuator: RS-555*4, RS-380*2, RZ-735*2, RS-385*2, PWM_Servo(KONDO)*2 */
+/* Actuator: RS-555*4, RS-385*4, RZ-735*2, PWM_Servo(KONDO)*2 */
/* Sensor: encorder*4, limit_switch*14 */
/* ------------------------------------------------------------------- */
/* Both of areas are compleated! */
@@ -18,6 +18,9 @@
#define RED 0
#define BLUE 1
+#define wind_time1 1.00f
+#define wind_time2 1.25f
+
//PID Gain of wheels(Kp, Ti, Td, control cycle)
//前進
PID front_migimae(4500000.0, 0.0, 0.0, 0.001);
@@ -99,8 +102,8 @@
//操作の段階変数
unsigned int phase = 0;
-int kaisyu_phase = 0;
-int tyokudo_phase = 0;
+unsigned int kaisyu_phase = 0;
+unsigned int tyokudo_phase = 0;
unsigned int start_zone = 1;
bool zone = RED;
@@ -170,10 +173,11 @@
void read_limit(void);
void wheel_reset(void);
void kaisyu(int pulse, int next_phase);
-void kaisyu_nobasu(int pulse, int next_phase);
-void kaisyu_hiku(int pulse, int next_phase);
+void kaisyu_nobasu(int next_phase);
+void kaisyu_hiku(int next_phase);
void tyokudo(int pulse, 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);
void back(int target);
void right(int target);
@@ -224,7 +228,14 @@
/*
if(start_switch == 1) {
- phase = 37;
+ //phase = 37;
+ if(zone == RED) {
+ phase = 35;
+ //phase = 37;
+ }
+ else if(zone == BLUE) {
+ phase = 37;
+ }
}
*/
@@ -254,7 +265,7 @@
//回収アームを伸ばす
case 1:
counter.reset();
- kaisyu_nobasu(arm_pulse, 2);
+ kaisyu_nobasu(2);
//サーボを開いておく
servo_data[0] = 0x03;
i2c.write(0x30, servo_data, 1);
@@ -275,7 +286,7 @@
//ちょっと前進
case 3:
counter.reset();
- front(800);
+ front(700);
if((y_pulse1 > 700) || (y_pulse2 > 700)) {
phase = 4;
}
@@ -297,21 +308,14 @@
//回収アーム引っ込める
case 5:
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;
- }
-
+ counter.reset();
+ kaisyu_hiku(6);
break;
//0.5秒停止
case 6:
stop();
+ USR_LED4 = 1;
counter.start();
if(counter.read() > 0.5f) {
phase = 7;
@@ -621,8 +625,8 @@
//掛けるところまで後進
case 33:
counter.reset();
- back(-9500);
- if((y_pulse1*-1 > 9500) || (y_pulse2*-1 > 9500)) {
+ back(-9200);
+ if((y_pulse1*-1 > 9200) || (y_pulse2*-1 > 9200)) {
phase = 34;
counter.start();
}
@@ -679,7 +683,9 @@
//シーツを掛ける
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;
@@ -705,6 +711,7 @@
i2c.write(0x30, servo_data, 1);
phase = FINAL_PHASE;
}
+ */
break;
//終了っ！(守衛さん風)
@@ -743,7 +750,7 @@
//回収アームを伸ばす
case 1:
counter.reset();
- kaisyu_nobasu(arm_pulse, 2);
+ kaisyu_nobasu(2);
//サーボを開いておく
servo_data[0] = 0x03;
i2c.write(0x30, servo_data, 1);
@@ -783,20 +790,12 @@
//回収アーム引っ込める
case 5:
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;
- }
- */
+ kaisyu_hiku(6);
break;
//左移動
case 6:
+ USR_LED4 = 1;
counter.reset();
left(11500);
if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
@@ -1144,7 +1143,10 @@
//シーツを掛ける
case 37:
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;
@@ -1170,6 +1172,7 @@
i2c.write(0x30, servo_data, 1);
phase = FINAL_PHASE;
}
+ */
break;
//終了っ！(守衛さん風)
@@ -1247,6 +1250,7 @@
}
void print_pulses(void) {
+
//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);
@@ -1572,10 +1576,12 @@
//回収MDへ書き込み
i2c.write(0x18, arm_motor, 1);
+ wait_us(20);
}
-void kaisyu_nobasu(int pulse, int next_phase) {
+void kaisyu_nobasu(int next_phase) {
+ /*
switch (kaisyu_phase) {
case 0:
//前進->減速
@@ -1599,8 +1605,8 @@
//3600pulse超えたら停止
else if(pulse >= 3600) {
arm_motor[0] = 0x80;
+ kaisyu_phase = 2;
phase = next_phase;
- kaisyu_phase = 2;
}
//後ろのリミットが押されたら強制停止
if(kaisyu_usiro_limit == 1) {
@@ -1614,12 +1620,39 @@
arm_motor[0] = 0x80;
break;
}
+ */
+
+ //前進->減速
+ //3000pulseまで高速前進
+ if(arm_pulse < 3000) {
+ arm_motor[0] = 0xFF;
+ }
+ //3000pulse超えたら低速前進
+ else if(arm_pulse >= 3000) {
+ arm_motor[0] = 0xB3;
+ }
+ //3600pulse超えたら停止
+ else if(arm_pulse >= 3600) {
+ 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へ書き込み
i2c.write(0x18, arm_motor, 1);
+ wait_us(20);
}
-void kaisyu_hiku(int pulse, int next_phase) {
+void kaisyu_hiku(int next_phase) {
+ /*
switch(kaisyu_phase) {
case 2:
//後進->減速
@@ -1653,8 +1686,34 @@
arm_motor[0] = 0x80;
break;
}
+ */
+
+ //後進->減速
+ //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へ書き込み
i2c.write(0x18, arm_motor, 1);
+ wait_us(20);
}
void tyokudo(int pulse, int next_phase) {
@@ -1728,6 +1787,7 @@
//回収MD・排出MDへ書き込み
i2c.write(0x18, arm_motor, 1);
i2c.write(0x20, drop_motor, 1);
+ wait_us(20);
}
void arm_up(int next_phase) {
@@ -1749,12 +1809,32 @@
right_arm_data[0] = 0x80; left_arm_data[0] = 0x80;
phase = next_phase;
}
-
i2c.write(0x22, right_arm_data, 1);
i2c.write(0x24, left_arm_data, 1);
wait_us(20);
}
+void fan_on(float first_wind_time, float second_wind_time, int next_phase) {
+
+ if(counter.read() <= first_wind_time) {
+ fan_data[0] = 0xFF;
+ servo_data[0] = 0x04;
+ }
+ else if((counter.read() > first_wind_time) && (counter.read() <= first_wind_time + second_wind_time)) {
+ fan_data[0] = 0xFF;
+ servo_data[0] = 0x03;
+ }
+ else if(counter.read() > first_wind_time + second_wind_time) {
+ fan_data[0] = 0x80;
+ servo_data[0] = 0x04;
+ phase = next_phase;
+ }
+ i2c.write(0x26, fan_data, 1);
+ i2c.write(0x28, fan_data, 1);
+ i2c.write(0x30, servo_data, 1);
+ wait_us(20);
+}
+
void front(int target) {
front_PID(target);