connection_readline() を追加。未テスト
@@ -1,23 +0,0 @@ | ||
1 | -/*! | |
2 | - \file | |
3 | - \brief 通信モジュールの初期化 | |
4 | - | |
5 | - \author Satofumi KAMIMURA | |
6 | - | |
7 | - $Id$ | |
8 | -*/ | |
9 | - | |
10 | -#include "communication_handler.h" | |
11 | -#include "sci_read_write.h" | |
12 | -#include "interrupt_priority.h" | |
13 | - | |
14 | -enum { | |
15 | - SCI_BAUDRATE = 38400, | |
16 | -}; | |
17 | - | |
18 | - | |
19 | -void communication_initialize(void) | |
20 | -{ | |
21 | - sci_initialize(INTERRUPT_PRIORITY_COMMUNICATION, SCI_BAUDRATE); | |
22 | - // !!! | |
23 | -} |
@@ -1,23 +0,0 @@ | ||
1 | -#ifndef QRK_COMMUNICATION_HANDLER_H | |
2 | -#define QRK_COMMUNICATION_HANDLER_H | |
3 | - | |
4 | -/*! | |
5 | - \file | |
6 | - \brief 通信モジュールの初期化 | |
7 | - | |
8 | - \author Satofumi KAMIMURA | |
9 | - | |
10 | - $Id$ | |
11 | -*/ | |
12 | - | |
13 | - | |
14 | -//! 初期化 | |
15 | -extern void communication_initialize(void); | |
16 | - | |
17 | -// !!! write | |
18 | - | |
19 | -// !!! flush | |
20 | - | |
21 | -// !!! read | |
22 | - | |
23 | -#endif /* !QRK_COMMUNICATION_HANDLER_H */ |
@@ -0,0 +1,28 @@ | ||
1 | +#ifndef QRK_SYSTEM_T_H | |
2 | +#define QRK_SYSTEM_T_H | |
3 | + | |
4 | +/*! | |
5 | + \file | |
6 | + \brief システム | |
7 | + | |
8 | + \author Satofumi KAMIMURA | |
9 | + | |
10 | + $Id$ | |
11 | +*/ | |
12 | + | |
13 | + | |
14 | +typedef enum { | |
15 | + NORMAL_CONTROL, //!< 通常動作 | |
16 | + DIRECT_WHEEL_CONTROL, //!< 車輪速の直接制御 | |
17 | +} control_mode_t; | |
18 | + | |
19 | + | |
20 | +typedef struct | |
21 | +{ | |
22 | + short msec; | |
23 | + long sec; | |
24 | + | |
25 | + control_mode_t mode; | |
26 | +} run_system_t; | |
27 | + | |
28 | +#endif /* !QRK_SYSTEM_T_H */ |
@@ -19,3 +19,17 @@ | ||
19 | 19 | } |
20 | 20 | return p - s; |
21 | 21 | } |
22 | + | |
23 | + | |
24 | +void *memcpy(void *dest, const void *src, size_t n) | |
25 | +{ | |
26 | + char *p = (char *)dest; | |
27 | + const char *q = (const char *)src; | |
28 | + const char *q_last = q + n; | |
29 | + | |
30 | + while (q < q_last) { | |
31 | + *p++ = *q++; | |
32 | + } | |
33 | + | |
34 | + return dest; | |
35 | +} |
@@ -70,10 +70,10 @@ | ||
70 | 70 | } |
71 | 71 | |
72 | 72 | |
73 | -int sci_read(char *data, int max_size) | |
73 | +int sci_read(char *data, int max_data_size) | |
74 | 74 | { |
75 | 75 | int i; |
76 | - for (i = 0; i < max_size; ++i) { | |
76 | + for (i = 0; i < max_data_size; ++i) { | |
77 | 77 | while (1) { |
78 | 78 | // SCSSR の ORER, PER, FER を読み出す |
79 | 79 | if (SCI1.SCSSR.BYTE & 0x38) { |
@@ -14,5 +14,6 @@ | ||
14 | 14 | |
15 | 15 | |
16 | 16 | extern size_t strlen(const char *s); |
17 | +extern void *memcpy(void *dest, const void *src, size_t n); | |
17 | 18 | |
18 | 19 | #endif /* !STD_STRING_H */ |
@@ -22,6 +22,6 @@ | ||
22 | 22 | |
23 | 23 | !!! 割り込み版では、受信済みのデータを格納して、即座に戻る。データがなかった場合は 0 を返すようにする |
24 | 24 | */ |
25 | -extern int sci_read(char *data, int max_size); | |
25 | +extern int sci_read(char *data, int max_data_size); | |
26 | 26 | |
27 | 27 | #endif /* !QRK_SCI_READ_WRITE_H */ |
@@ -28,45 +28,47 @@ | ||
28 | 28 | |
29 | 29 | .PHONY : all clean html upload |
30 | 30 | ###################################################################### |
31 | -run_drive.mot : run_drive.o initialize_state.o control_state.o pause_state.o clock_initialize.o imask.o communication_handler.o protocol_handler.o sci_interrupt_read_write.o emergency_io_control.o timer_control.o path_follow.o position_handler.o velocity_handler.o body_handler.o wheel_velocity.o motor_velocity.o motor_pwm.o encoder_reader.o odometry_calculate.o mtu2_initialize.o ring_buffer.o | |
31 | +run_drive.mot : run_drive.o initialize_state.o control_state.o pause_state.o clock_initialize.o imask.o run_system.o connection.o connection_utilities.o protocol_handler.o sci_interrupt_read_write.o emergency_io_control.o timer_control.o path_follow.o position_handler.o velocity_handler.o body_handler.o wheel_velocity.o motor_velocity.o motor_pwm.o encoder_reader.o odometry_calculate.o mtu2_initialize.o ring_buffer.o | |
32 | 32 | |
33 | 33 | # DO NOT DELETE |
34 | 34 | |
35 | 35 | body_handler.o: wheel_t.h encoder_t.h motor_t.h |
36 | -control_state.o: run_t.h controller_config.h odometry_t.h path_t.h wheel_t.h | |
37 | -control_state.o: encoder_t.h motor_t.h | |
36 | +control_state.o: run_t.h controller_config.h run_system_t.h odometry_t.h | |
37 | +control_state.o: path_t.h wheel_t.h encoder_t.h motor_t.h | |
38 | 38 | encoder_reader.o: encoder_t.h |
39 | -initialize_state.o: run_t.h controller_config.h odometry_t.h path_t.h | |
40 | -initialize_state.o: wheel_t.h encoder_t.h motor_t.h | |
39 | +initialize_state.o: run_t.h controller_config.h run_system_t.h odometry_t.h | |
40 | +initialize_state.o: path_t.h wheel_t.h encoder_t.h motor_t.h | |
41 | 41 | motor_velocity.o: motor_t.h |
42 | 42 | odometry_calculate.o: odometry_t.h |
43 | 43 | path_follow.o: path_t.h |
44 | -pause_state.o: run_t.h controller_config.h odometry_t.h path_t.h wheel_t.h | |
45 | -pause_state.o: encoder_t.h motor_t.h | |
46 | -protocol_handler.o: run_t.h controller_config.h odometry_t.h path_t.h | |
47 | -protocol_handler.o: wheel_t.h encoder_t.h motor_t.h | |
48 | -run_t.o: controller_config.h odometry_t.h path_t.h wheel_t.h encoder_t.h | |
49 | -run_t.o: motor_t.h | |
44 | +pause_state.o: run_t.h controller_config.h run_system_t.h odometry_t.h | |
45 | +pause_state.o: path_t.h wheel_t.h encoder_t.h motor_t.h | |
46 | +protocol_handler.o: run_t.h controller_config.h run_system_t.h odometry_t.h | |
47 | +protocol_handler.o: path_t.h wheel_t.h encoder_t.h motor_t.h | |
48 | +run_system.o: run_system_t.h | |
49 | +run_t.o: controller_config.h run_system_t.h odometry_t.h path_t.h wheel_t.h | |
50 | +run_t.o: encoder_t.h motor_t.h | |
50 | 51 | wheel_t.o: encoder_t.h motor_t.h |
51 | 52 | wheel_velocity.o: wheel_t.h encoder_t.h motor_t.h |
52 | 53 | body_handler.o: body_handler.h wheel_t.h encoder_t.h motor_t.h |
53 | 54 | body_handler.o: wheel_velocity.h controller_config.h robot_parameter.h |
54 | 55 | clock_initialize.o: clock_initialize.h |
55 | -communication_handler.o: communication_handler.h sci_read_write.h | |
56 | -communication_handler.o: interrupt_priority.h | |
57 | -control_state.o: control_state.h run_t.h controller_config.h odometry_t.h | |
58 | -control_state.o: path_t.h wheel_t.h encoder_t.h motor_t.h imask.h | |
59 | -control_state.o: interrupt_priority.h pause_state.h emergency_io_control.h | |
60 | -control_state.o: timer_control.h protocol_handler.h encoder_reader.h | |
61 | -control_state.o: odometry_calculate.h path_follow.h wheel_velocity.h | |
56 | +connection.o: connection.h sci_read_write.h interrupt_priority.h | |
57 | +control_state.o: control_state.h run_t.h controller_config.h run_system_t.h | |
58 | +control_state.o: odometry_t.h path_t.h wheel_t.h encoder_t.h motor_t.h | |
59 | +control_state.o: imask.h interrupt_priority.h pause_state.h | |
60 | +control_state.o: emergency_io_control.h timer_control.h protocol_handler.h | |
61 | +control_state.o: encoder_reader.h odometry_calculate.h path_follow.h | |
62 | +control_state.o: wheel_velocity.h | |
62 | 63 | emergency_io_control.o: emergency_io_control.h |
63 | 64 | encoder_reader.o: encoder_reader.h encoder_t.h mtu2_initialize.h |
64 | 65 | imask.o: imask.h |
65 | 66 | initialize_state.o: initialize_state.h run_t.h controller_config.h |
66 | -initialize_state.o: odometry_t.h path_t.h wheel_t.h encoder_t.h motor_t.h | |
67 | -initialize_state.o: imask.h clock_initialize.h timer_control.h | |
68 | -initialize_state.o: emergency_io_control.h odometry_calculate.h path_follow.h | |
69 | -initialize_state.o: wheel_velocity.h protocol_handler.h interrupt_priority.h | |
67 | +initialize_state.o: run_system_t.h odometry_t.h path_t.h wheel_t.h | |
68 | +initialize_state.o: encoder_t.h motor_t.h imask.h clock_initialize.h | |
69 | +initialize_state.o: timer_control.h run_system.h emergency_io_control.h | |
70 | +initialize_state.o: odometry_calculate.h path_follow.h wheel_velocity.h | |
71 | +initialize_state.o: protocol_handler.h interrupt_priority.h | |
70 | 72 | motor_pwm.o: motor_pwm.h mtu2_initialize.h |
71 | 73 | motor_velocity.o: motor_velocity.h motor_t.h robot_parameter.h motor_pwm.h |
72 | 74 | motor_velocity.o: encoder_reader.h encoder_t.h |
@@ -73,16 +75,18 @@ | ||
73 | 75 | mtu2_initialize.o: mtu2_initialize.h |
74 | 76 | odometry_calculate.o: odometry_calculate.h odometry_t.h |
75 | 77 | path_follow.o: path_t.h position_handler.h |
76 | -pause_state.o: pause_state.h run_t.h controller_config.h odometry_t.h | |
77 | -pause_state.o: path_t.h wheel_t.h encoder_t.h motor_t.h | |
78 | +pause_state.o: pause_state.h run_t.h controller_config.h run_system_t.h | |
79 | +pause_state.o: odometry_t.h path_t.h wheel_t.h encoder_t.h motor_t.h | |
78 | 80 | pause_state.o: emergency_io_control.h wheel_velocity.h |
79 | 81 | position_handler.o: position_handler.h velocity_handler.h |
80 | 82 | protocol_handler.o: protocol_handler.h run_t.h controller_config.h |
81 | -protocol_handler.o: odometry_t.h path_t.h wheel_t.h encoder_t.h motor_t.h | |
82 | -protocol_handler.o: communication_handler.h | |
83 | +protocol_handler.o: run_system_t.h odometry_t.h path_t.h wheel_t.h | |
84 | +protocol_handler.o: encoder_t.h motor_t.h connection.h | |
83 | 85 | ring_buffer.o: ring_buffer.h |
84 | -run_drive.o: initialize_state.h run_t.h controller_config.h odometry_t.h | |
85 | -run_drive.o: path_t.h wheel_t.h encoder_t.h motor_t.h control_state.h | |
86 | +run_drive.o: initialize_state.h run_t.h controller_config.h run_system_t.h | |
87 | +run_drive.o: odometry_t.h path_t.h wheel_t.h encoder_t.h motor_t.h | |
88 | +run_drive.o: control_state.h | |
89 | +run_system.o: run_system.h run_system_t.h | |
86 | 90 | sci_interrupt_read_write.o: sci_read_write.h imask.h cpu_clock.h |
87 | 91 | sci_interrupt_read_write.o: ring_buffer.h |
88 | 92 | sci_read_write.o: sci_read_write.h cpu_clock.h |
@@ -1,6 +1,6 @@ | ||
1 | 1 | # taiyaki/dox |
2 | 2 | |
3 | 3 | all : |
4 | - cd ../../ && $(MAKE) html | |
4 | + cd ../../../ && $(MAKE) html | |
5 | 5 | |
6 | 6 | clean : |
\ No newline at end of file |
@@ -11,6 +11,7 @@ | ||
11 | 11 | */ |
12 | 12 | |
13 | 13 | #include "controller_config.h" |
14 | +#include "run_system_t.h" | |
14 | 15 | #include "odometry_t.h" |
15 | 16 | #include "path_t.h" |
16 | 17 | #include "wheel_t.h" |
@@ -18,6 +19,7 @@ | ||
18 | 19 | |
19 | 20 | //! 走行状態の管理 |
20 | 21 | typedef struct { |
22 | + run_system_t run_system; | |
21 | 23 | odometry_t odometry; |
22 | 24 | path_t path; |
23 | 25 | wheel_t wheel[NUMBER_OF_WHEELS]; |
@@ -8,12 +8,34 @@ | ||
8 | 8 | */ |
9 | 9 | |
10 | 10 | #include "protocol_handler.h" |
11 | -#include "communication_handler.h" | |
11 | +#include "connection.h" | |
12 | 12 | |
13 | 13 | |
14 | +typedef enum { | |
15 | + // !!! | |
16 | + WV_Command, //!< 車輪速の制御 | |
17 | +} command_type_t; | |
18 | + | |
19 | + | |
20 | +typedef struct | |
21 | +{ | |
22 | + command_type_t type; | |
23 | + const char *tag; | |
24 | + char tag_size; | |
25 | + char packet_size; | |
26 | +} command_packet_t; | |
27 | + | |
28 | + | |
29 | +static command_packet_t command_packets[] = { | |
30 | + // !!! | |
31 | + { WV_Command, "WV", 2, 5, }, | |
32 | +}; | |
33 | + | |
34 | + | |
14 | 35 | void protocol_initialize(void) |
15 | 36 | { |
16 | - communication_initialize(); | |
37 | + connection_initialize(); | |
38 | + | |
17 | 39 | // !!! |
18 | 40 | } |
19 | 41 |
@@ -20,6 +42,18 @@ | ||
20 | 42 | |
21 | 43 | void protocol_update(run_t *run) |
22 | 44 | { |
45 | + int packets_size = sizeof(command_packets) / sizeof(command_packets[0]); | |
46 | + int i; | |
47 | + | |
48 | + for (i = 0; i < packets_size; ++i) { | |
49 | + // !!! | |
50 | + } | |
51 | + | |
23 | 52 | (void)run; |
24 | 53 | // !!! |
54 | + | |
55 | + (void)command_packets; | |
56 | + | |
57 | + // 車輪速の制御を受け付けるようにする | |
58 | + // !!! | |
25 | 59 | } |
@@ -11,6 +11,7 @@ | ||
11 | 11 | #include "imask.h" |
12 | 12 | #include "clock_initialize.h" |
13 | 13 | #include "timer_control.h" |
14 | +#include "run_system.h" | |
14 | 15 | #include "emergency_io_control.h" |
15 | 16 | #include "odometry_calculate.h" |
16 | 17 | #include "path_follow.h" |
@@ -27,9 +28,9 @@ | ||
27 | 28 | imask_initialize(); |
28 | 29 | clock_initialize(); |
29 | 30 | timer_initialize(INTERRUPT_PRIORITY_TIMER); |
30 | - // !!! | |
31 | 31 | |
32 | 32 | // 走行モジュール |
33 | + run_system_initialize(&run->run_system); | |
33 | 34 | emergency_io_initialize(); |
34 | 35 | odometry_initialize(&run->odometry); |
35 | 36 | path_follow_initialize(&run->path); |
@@ -0,0 +1,18 @@ | ||
1 | +/*! | |
2 | + \file | |
3 | + \brief 走行制御のシステム管理 | |
4 | + | |
5 | + \author Satofumi KAMIMURA | |
6 | + | |
7 | + $Id$ | |
8 | +*/ | |
9 | + | |
10 | +#include "run_system.h" | |
11 | + | |
12 | + | |
13 | +void run_system_initialize(run_system_t *run_system) | |
14 | +{ | |
15 | + run_system->msec = 0; | |
16 | + run_system->sec = 0; | |
17 | + run_system->mode = NORMAL_CONTROL; | |
18 | +} |
@@ -0,0 +1,19 @@ | ||
1 | +#ifndef QRK_RUN_SYSTEM_H | |
2 | +#define QRK_RUN_SYSTEM_H | |
3 | + | |
4 | +/*! | |
5 | + \file | |
6 | + \brief 走行制御のシステム管理 | |
7 | + | |
8 | + \author Satofumi KAMIMURA | |
9 | + | |
10 | + $Id$ | |
11 | +*/ | |
12 | + | |
13 | +#include "run_system_t.h" | |
14 | + | |
15 | + | |
16 | +extern void run_system_initialize(run_system_t *run_system); | |
17 | +// !!! | |
18 | + | |
19 | +#endif /* !QRK_RUN_SYSTEM_H */ |
@@ -0,0 +1,55 @@ | ||
1 | +/*! | |
2 | + \file | |
3 | + \brief 通信モジュールの補助関数 | |
4 | + | |
5 | + \author Satofumi KAMIMURA | |
6 | + | |
7 | + $Id$ | |
8 | +*/ | |
9 | + | |
10 | +#include "connection_utilities.h" | |
11 | +#include "connection.h" | |
12 | +#include "std_string.h" | |
13 | + | |
14 | + | |
15 | +int connection_readline(char *data, int max_data_size) | |
16 | +{ | |
17 | + enum { | |
18 | + LINE_BUFFER_SIZE = 128, | |
19 | + }; | |
20 | + static char line_buffer_[LINE_BUFFER_SIZE]; | |
21 | + static int line_buffer_filled_ = 0; | |
22 | + char ch; | |
23 | + int n; | |
24 | + | |
25 | + while (1) { | |
26 | + n = connection_read(&ch, 1); | |
27 | + if (n <= 0) { | |
28 | + return -1; | |
29 | + } | |
30 | + | |
31 | + if (ch == '\n') { | |
32 | + if (line_buffer_filled_ <= 0) { | |
33 | + line_buffer_filled_ = 0; | |
34 | + return 0; | |
35 | + } else { | |
36 | + int ret_value = line_buffer_filled_; | |
37 | + memcpy(data, line_buffer_, line_buffer_filled_); | |
38 | + data[line_buffer_filled_] = '\n'; | |
39 | + line_buffer_filled_ = 0; | |
40 | + return ret_value; | |
41 | + } | |
42 | + } | |
43 | + | |
44 | + // 格納 | |
45 | + line_buffer_[line_buffer_filled_++] = ch; | |
46 | + | |
47 | + // 最大サイズまでデータを格納したら戻る | |
48 | + if (line_buffer_filled_ >= max_data_size) { | |
49 | + int ret_value = line_buffer_filled_; | |
50 | + memcpy(data, line_buffer_, line_buffer_filled_); | |
51 | + line_buffer_filled_ = 0; | |
52 | + return ret_value; | |
53 | + } | |
54 | + } | |
55 | +} |
@@ -0,0 +1,16 @@ | ||
1 | +#ifndef QRK_CONNECTION_UTILITIES_H | |
2 | +#define QRK_CONNECTION_UTILITIES_H | |
3 | + | |
4 | +/*! | |
5 | + \file | |
6 | + \brief 通信モジュールの補助関数 | |
7 | + | |
8 | + \author Satofumi KAMIMURA | |
9 | + | |
10 | + $Id$ | |
11 | +*/ | |
12 | + | |
13 | + | |
14 | +extern int connection_readline(char *data, int max_data_size); | |
15 | + | |
16 | +#endif /* !QRK_CONNECTION_UTILITIES_H */ |
@@ -0,0 +1,40 @@ | ||
1 | +/*! | |
2 | + \file | |
3 | + \brief 通信モジュール | |
4 | + | |
5 | + \author Satofumi KAMIMURA | |
6 | + | |
7 | + $Id$ | |
8 | +*/ | |
9 | + | |
10 | +#include "connection.h" | |
11 | +#include "sci_read_write.h" | |
12 | +#include "interrupt_priority.h" | |
13 | + | |
14 | +enum { | |
15 | + SCI_BAUDRATE = 38400, | |
16 | +}; | |
17 | + | |
18 | + | |
19 | +void connection_initialize(void) | |
20 | +{ | |
21 | + sci_initialize(INTERRUPT_PRIORITY_COMMUNICATION, SCI_BAUDRATE); | |
22 | +} | |
23 | + | |
24 | + | |
25 | +int connection_write(const char *data, int data_size) | |
26 | +{ | |
27 | + return sci_write(data, data_size); | |
28 | +} | |
29 | + | |
30 | + | |
31 | +void connection_write_flush(void) | |
32 | +{ | |
33 | + // !!! 未実装 | |
34 | +} | |
35 | + | |
36 | + | |
37 | +int connection_read(char *data, int max_data_size) | |
38 | +{ | |
39 | + return sci_read(data, max_data_size); | |
40 | +} |
@@ -96,13 +96,13 @@ | ||
96 | 96 | } |
97 | 97 | |
98 | 98 | |
99 | -int sci_read(char *data, int max_size) | |
99 | +int sci_read(char *data, int max_data_size) | |
100 | 100 | { |
101 | 101 | int current_level = get_imask_exr(); |
102 | 102 | int read_size; |
103 | 103 | |
104 | 104 | set_imask_exr(interrupt_priority_); |
105 | - read_size = ring_read(&read_ring_, data, max_size); | |
105 | + read_size = ring_read(&read_ring_, data, max_data_size); | |
106 | 106 | set_imask_exr(current_level); |
107 | 107 | |
108 | 108 | return read_size; |
@@ -0,0 +1,29 @@ | ||
1 | +#ifndef QRK_COMMUNICATION_HANDLER_H | |
2 | +#define QRK_COMMUNICATION_HANDLER_H | |
3 | + | |
4 | +/*! | |
5 | + \file | |
6 | + \brief 通信モジュール | |
7 | + | |
8 | + \author Satofumi KAMIMURA | |
9 | + | |
10 | + $Id$ | |
11 | +*/ | |
12 | + | |
13 | + | |
14 | +//! 初期化 | |
15 | +extern void connection_initialize(void); | |
16 | + | |
17 | + | |
18 | +// !!! write | |
19 | +extern int connection_write(const char *data, int data_size); | |
20 | + | |
21 | + | |
22 | +// !!! flush | |
23 | +extern void connection_write_flush(void); | |
24 | + | |
25 | + | |
26 | +// !!! read | |
27 | +extern int connection_read(char *data, int max_data_size); | |
28 | + | |
29 | +#endif /* !QRK_COMMUNICATION_HANDLER_H */ |
@@ -10,6 +10,7 @@ | ||
10 | 10 | #include "control_state.h" |
11 | 11 | #include "imask.h" |
12 | 12 | #include "interrupt_priority.h" |
13 | +#include "robot_parameter.h" | |
13 | 14 | #include "pause_state.h" |
14 | 15 | #include "emergency_io_control.h" |
15 | 16 | #include "timer_control.h" |
@@ -26,11 +27,24 @@ | ||
26 | 27 | |
27 | 28 | static void control_task(void) |
28 | 29 | { |
30 | + static int msec = 0; | |
29 | 31 | int left_encoder_count; |
30 | 32 | int right_encoder_count; |
31 | 33 | int i; |
32 | 34 | unsigned char current_interrupt_priority = get_imask_exr(); |
33 | 35 | |
36 | + // CONTROL_CYCLE_MSEC 毎に処理を行う | |
37 | + if (++msec >= CONTROL_CYCLE_MSEC) { | |
38 | + msec = 0; | |
39 | + } else { | |
40 | + return; | |
41 | + } | |
42 | + | |
43 | + // タイムスタンプの更新 | |
44 | + // !!! | |
45 | + // !!! 関数化する | |
46 | + //++run_->system.msec; | |
47 | + | |
34 | 48 | // エンコーダ値の更新 |
35 | 49 | set_imask_exr(INTERRUPT_PRIORITY_ALL_MASK); |
36 | 50 | for (i = 0; i < NUMBER_OF_WHEELS; ++i) { |
@@ -43,9 +57,17 @@ | ||
43 | 57 | right_encoder_count = encoder_difference(&run_->wheel[RIGHT_WHEEL].encoder); |
44 | 58 | odometry_update(&run_->odometry, left_encoder_count, right_encoder_count); |
45 | 59 | |
46 | - // 経路制御 | |
47 | - path_follow_update(&run_->path); | |
60 | + // 走行経路の指示 | |
61 | + switch (run_->run_system.mode) { | |
62 | + case NORMAL_CONTROL: | |
63 | + path_follow_update(&run_->path); | |
64 | + break; | |
48 | 65 | |
66 | + case DIRECT_WHEEL_CONTROL: | |
67 | + // 何もしない | |
68 | + break; | |
69 | + } | |
70 | + | |
49 | 71 | // 設定された車輪速になるように制御する |
50 | 72 | for (i = 0; i < NUMBER_OF_WHEELS; ++i) { |
51 | 73 | wheel_velocity_control(&run_->wheel[i]); |
@@ -61,6 +61,9 @@ | ||
61 | 61 | motor_velocity_run.o: ../encoder_t.h |
62 | 62 | sci_echoback.o: ../clock_initialize.h ../imask.h ../sci_read_write.h |
63 | 63 | sci_echoback.o: ../interrupt_priority.h |
64 | +sci_interrupt_echoback.o: ../clock_initialize.h ../imask.h | |
65 | +sci_interrupt_echoback.o: ../sci_interrupt_read_write.h | |
66 | +sci_interrupt_echoback.o: ../interrupt_priority.h | |
64 | 67 | sci_interrupt_write.o: ../clock_initialize.h ../imask.h |
65 | 68 | sci_interrupt_write.o: ../sci_interrupt_read_write.h ../sci_utilities.h |
66 | 69 | sci_interrupt_write.o: ../interrupt_priority.h |