• R/O
  • SSH
  • HTTPS

qrobosdk: コミット


コミットメタ情報

リビジョン1920 (tree)
日時2010-09-19 02:04:41
作者satofumi

ログメッセージ

モータ制御を調整

変更サマリ

差分

--- branches/tsukuba_2010/libs/run_drive/motor_velocity.c (revision 1919)
+++ branches/tsukuba_2010/libs/run_drive/motor_velocity.c (revision 1920)
@@ -14,7 +14,7 @@
1414
1515 enum {
1616 DEFAULT_GAIN_P = 32, /* 1/1 の重さ */
17- DEFAULT_GAIN_I = 32 * CONTROL_CYCLE_MSEC_SHIFT, /* 1/256 の重さ */
17+ DEFAULT_GAIN_I = 32, /* 1/256 の重さ */
1818 };
1919
2020
@@ -67,5 +67,12 @@
6767 output = 0;
6868 }
6969
70+#if 0
71+ // !!! リミットを付加
72+ if (output > 153) {
73+ output = 153;
74+ }
75+#endif
76+
7077 motor_pwm_set_duty(motor->device_id, direction, output);
7178 }
--- branches/tsukuba_2010/libs/run_drive/std_string.c (revision 1919)
+++ branches/tsukuba_2010/libs/run_drive/std_string.c (revision 1920)
@@ -33,3 +33,18 @@
3333
3434 return dest;
3535 }
36+
37+
38+int strncmp(const char *s1, const char *s2, size_t n)
39+{
40+ size_t i;
41+
42+ for (i = 0; i < n; ++i) {
43+ if (*s1 != *s2) {
44+ return *s1 - *s2;
45+ }
46+ ++s1;
47+ ++s2;
48+ }
49+ return 0;
50+}
--- branches/tsukuba_2010/libs/run_drive/std_string.h (revision 1919)
+++ branches/tsukuba_2010/libs/run_drive/std_string.h (revision 1920)
@@ -15,5 +15,6 @@
1515
1616 extern size_t strlen(const char *s);
1717 extern void *memcpy(void *dest, const void *src, size_t n);
18+extern int strncmp(const char *s1, const char *s2, size_t n);
1819
1920 #endif /* !STD_STRING_H */
--- branches/tsukuba_2010/libs/run_drive/Makefile (revision 1919)
+++ branches/tsukuba_2010/libs/run_drive/Makefile (revision 1920)
@@ -4,7 +4,7 @@
44 SOURCE_DIR = .
55 include $(SOURCE_DIR)/build_rule.mk
66
7-CFLAGS_FOR_BUILD = -DHOST_COMPILE -m2 -Wall -Werror -W $(INCLUDES_FOR_BUILD)
7+CFLAGS_FOR_BUILD = -DHOST_COMPILE -O2 -m2 -Wall -Werror -W $(INCLUDES_FOR_BUILD)
88 INCLUDES_FOR_BUILD =
99 LDFLAGS_FOR_BUILD = -nostartfiles $(START_UP_FOR_BUILD)
1010 LDLIBS_FOR_BUILD =
@@ -28,7 +28,7 @@
2828
2929 .PHONY : all clean html upload
3030 ######################################################################
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
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 run_commands.o htoi.o
3232
3333 # DO NOT DELETE
3434
@@ -82,7 +82,8 @@
8282 position_handler.o: position_handler.h velocity_handler.h
8383 protocol_handler.o: protocol_handler.h run_t.h controller_config.h
8484 protocol_handler.o: run_system_t.h odometry_t.h path_t.h wheel_t.h
85-protocol_handler.o: encoder_t.h motor_t.h connection.h
85+protocol_handler.o: encoder_t.h motor_t.h connection.h connection_utilities.h
86+protocol_handler.o: std_string.h
8687 ring_buffer.o: ring_buffer.h
8788 run_drive.o: initialize_state.h run_t.h controller_config.h run_system_t.h
8889 run_drive.o: odometry_t.h path_t.h wheel_t.h encoder_t.h motor_t.h
--- branches/tsukuba_2010/libs/run_drive/protocol_handler.c (revision 1919)
+++ branches/tsukuba_2010/libs/run_drive/protocol_handler.c (revision 1920)
@@ -9,9 +9,13 @@
99
1010 #include "protocol_handler.h"
1111 #include "connection.h"
12+#include "connection_utilities.h"
13+#include "run_commands.h"
14+#include "std_string.h"
1215
1316
1417 typedef enum {
18+ Unknown_Command,
1519 // !!!
1620 WV_Command, //!< 車輪速の制御
1721 } command_type_t;
@@ -26,7 +30,7 @@
2630 } command_packet_t;
2731
2832
29-static command_packet_t command_packets[] = {
33+static command_packet_t command_packets_[] = {
3034 // !!!
3135 { WV_Command, "WV", 2, 5, },
3236 };
@@ -40,20 +44,46 @@
4044 }
4145
4246
43-void protocol_update(run_t *run)
47+command_type_t parse_received_command(const char *line_buffer, int line_size)
4448 {
45- int packets_size = sizeof(command_packets) / sizeof(command_packets[0]);
49+ int packets_size = sizeof(command_packets_) / sizeof(command_packets_[0]);
4650 int i;
4751
4852 for (i = 0; i < packets_size; ++i) {
49- // !!!
53+ const command_packet_t *p = &command_packets_[i];
54+ if (line_size != p->packet_size) {
55+ continue;
56+ }
57+
58+ if (strncmp(p->tag, line_buffer, p->tag_size)) {
59+ continue;
60+ }
61+
62+ // コマンドのタグが一致
63+ return p->type;
5064 }
65+ return Unknown_Command;
66+}
5167
52- (void)run;
53- // !!!
5468
55- (void)command_packets;
69+void protocol_update(run_t *run)
70+{
71+ char line_buffer[CONNECTION_READLINE_BUFFER_SIZE];
72+ int line_size;
73+ command_type_t command_type;
5674
57- // 車輪速の制御を受け付けるようにする
58- // !!!
75+ line_size =
76+ connection_readline(line_buffer, CONNECTION_READLINE_BUFFER_SIZE);
77+ command_type = parse_received_command(line_buffer, line_size);
78+
79+ // コマンドのパース処理
80+ switch (command_type) {
81+ case WV_Command:
82+ handle_WV_command(run, line_buffer);
83+ break;
84+
85+ case Unknown_Command:
86+ // !!! エラー応答を返す
87+ break;
88+ }
5989 }
--- branches/tsukuba_2010/libs/run_drive/connection_utilities.c (revision 1919)
+++ branches/tsukuba_2010/libs/run_drive/connection_utilities.c (revision 1920)
@@ -14,10 +14,7 @@
1414
1515 int connection_readline(char *data, int max_data_size)
1616 {
17- enum {
18- LINE_BUFFER_SIZE = 128,
19- };
20- static char line_buffer_[LINE_BUFFER_SIZE];
17+ static char line_buffer_[CONNECTION_READLINE_BUFFER_SIZE];
2118 static int line_buffer_filled_ = 0;
2219 char ch;
2320 int n;
--- branches/tsukuba_2010/libs/run_drive/connection_utilities.h (revision 1919)
+++ branches/tsukuba_2010/libs/run_drive/connection_utilities.h (revision 1920)
@@ -11,6 +11,11 @@
1111 */
1212
1313
14+enum {
15+ CONNECTION_READLINE_BUFFER_SIZE = 128,
16+};
17+
18+
1419 extern int connection_readline(char *data, int max_data_size);
1520
1621 #endif /* !QRK_CONNECTION_UTILITIES_H */
--- branches/tsukuba_2010/libs/run_drive/htoi.c (nonexistent)
+++ branches/tsukuba_2010/libs/run_drive/htoi.c (revision 1920)
@@ -0,0 +1,31 @@
1+/*!
2+ \file
3+ \brief 16進文字列の数値への変換
4+
5+ \author Satofumi KAMIMURA
6+
7+ $Id$
8+*/
9+
10+#include "htoi.h"
11+
12+
13+unsigned long htoi(const char *hex_data, int data_size)
14+{
15+ int i;
16+ long value = 0;
17+
18+ for (i = 0; i < data_size; ++i) {
19+ char ch = hex_data[i];
20+ if ((ch >= '0') && (ch <= '9')) {
21+ value += ch - '0';
22+ } else if ((ch >= 'a') && (ch <= 'f')) {
23+ value += (ch - 'a') + 10;
24+ } else if ((ch >= 'A') && (ch <= 'F')) {
25+ value += (ch - 'A') + 10;
26+ }
27+ value <<= 4;
28+ }
29+
30+ return value;
31+}
Added: svn:keywords
## -0,0 +1 ##
+Id
\ No newline at end of property
--- branches/tsukuba_2010/libs/run_drive/htoi.h (nonexistent)
+++ branches/tsukuba_2010/libs/run_drive/htoi.h (revision 1920)
@@ -0,0 +1,16 @@
1+#ifndef QRK_HTOI_H
2+#define QRK_HTOI_H
3+
4+/*!
5+ \file
6+ \brief 16進文字列の数値への変換
7+
8+ \author Satofumi KAMIMURA
9+
10+ $Id$
11+*/
12+
13+
14+extern unsigned long htoi(const char *hex_data, int data_size);
15+
16+#endif /* !QRK_HTOI_H */
Added: svn:keywords
## -0,0 +1 ##
+Id
\ No newline at end of property
--- branches/tsukuba_2010/libs/run_drive/samples/motor_velocity_run.c (revision 1919)
+++ branches/tsukuba_2010/libs/run_drive/samples/motor_velocity_run.c (revision 1920)
@@ -12,6 +12,7 @@
1212 #include "motor_velocity.h"
1313 #include "timer_control.h"
1414 #include "encoder_reader.h"
15+#include "robot_parameter.h"
1516
1617
1718 static encoder_t encoder_[2];
@@ -19,7 +20,7 @@
1920
2021
2122 enum {
22- TARGET_COUNT = 4,
23+ TARGET_COUNT = 1,
2324 };
2425
2526
@@ -29,7 +30,7 @@
2930 int current_count[2];
3031 int i;
3132
32- if (++msec >= 4) {
33+ if (++msec >= CONTROL_CYCLE_MSEC) {
3334 msec = 0;
3435 } else {
3536 return;
@@ -40,7 +41,7 @@
4041 current_count[i] = encoder_difference(&encoder_[i]);
4142 }
4243 motor_velocity_control(&motor_[0], +TARGET_COUNT, current_count[0]);
43- motor_velocity_control(&motor_[1], +TARGET_COUNT, current_count[1]);
44+ motor_velocity_control(&motor_[1], -TARGET_COUNT, current_count[1]);
4445 }
4546
4647
--- branches/tsukuba_2010/libs/run_drive/samples/wheel_velocity_run.c (revision 1919)
+++ branches/tsukuba_2010/libs/run_drive/samples/wheel_velocity_run.c (revision 1920)
@@ -12,6 +12,7 @@
1212 #include "wheel_velocity.h"
1313 #include "encoder_reader.h"
1414 #include "timer_control.h"
15+#include "robot_parameter.h"
1516
1617
1718 static wheel_t wheel_[2];
@@ -22,7 +23,7 @@
2223 static int msec = 0;
2324 int i;
2425
25- if (++msec >= 4) {
26+ if (++msec >= CONTROL_CYCLE_MSEC) {
2627 msec = 0;
2728 } else {
2829 return;
@@ -42,7 +43,7 @@
4243 int main(void)
4344 {
4445 enum {
45- VELOCITY_MM_PER_SEC = 200,
46+ VELOCITY_MM_PER_SEC = 100,
4647 };
4748 int i;
4849
@@ -53,8 +54,9 @@
5354
5455 for (i = 0; i < 2; ++i) {
5556 wheel_initialize(&wheel_[i], i);
56- wheel_set_velocity(&wheel_[i], VELOCITY_MM_PER_SEC);
5757 }
58+ wheel_set_velocity(&wheel_[0], VELOCITY_MM_PER_SEC);
59+ wheel_set_velocity(&wheel_[1], -VELOCITY_MM_PER_SEC);
5860
5961 timer_set_interval_function(timer_handler);
6062 timer_start();
--- branches/tsukuba_2010/libs/run_drive/samples/motor_pwm_out.c (revision 1919)
+++ branches/tsukuba_2010/libs/run_drive/samples/motor_pwm_out.c (revision 1920)
@@ -13,7 +13,7 @@
1313 int main(void)
1414 {
1515 enum {
16- DUTY = 32,
16+ DUTY = 255 * 70 / 100,
1717 };
1818 int i;
1919
@@ -21,8 +21,9 @@
2121
2222 for (i = 0; i < 2; ++i) {
2323 motor_pwm_initialize(i);
24- motor_pwm_set_duty(i, MOTOR_PWM_CW, DUTY);
2524 }
25+ motor_pwm_set_duty(0, MOTOR_PWM_CW, DUTY);
26+ motor_pwm_set_duty(1, MOTOR_PWM_CCW, DUTY);
2627
2728 while (1) {
2829 ;
--- branches/tsukuba_2010/libs/run_drive/samples/encoder_print.c (revision 1919)
+++ branches/tsukuba_2010/libs/run_drive/samples/encoder_print.c (revision 1920)
@@ -12,6 +12,7 @@
1212 #include "encoder_reader.h"
1313 #include "sci_read_write.h"
1414 #include "sci_utilities.h"
15+#include "motor_pwm.h"
1516
1617
1718 static encoder_t encoder_[2];
@@ -48,6 +49,10 @@
4849
4950 int main(void)
5051 {
52+ enum {
53+ DUTY = 24,
54+ };
55+
5156 imask_initialize();
5257 clock_initialize();
5358 sci_initialize(INTERRUPT_PRIORITY_COMMUNICATION, 38400);
@@ -55,8 +60,11 @@
5560 int i;
5661 for (i = 0; i < 2; ++i) {
5762 encoder_initialize(&encoder_[i], i);
63+ motor_pwm_initialize(i);
64+ motor_pwm_set_duty(i, MOTOR_PWM_CW, DUTY);
5865 }
5966
67+
6068 while (1) {
6169 wait();
6270 output();
--- branches/tsukuba_2010/libs/run_drive/samples/Makefile (revision 1919)
+++ branches/tsukuba_2010/libs/run_drive/samples/Makefile (revision 1920)
@@ -4,7 +4,7 @@
44 SOURCE_DIR = ..
55 include $(SOURCE_DIR)/build_rule.mk
66
7-CFLAGS_FOR_BUILD = -DHOST_COMPILE -m2 -Wall -Werror -W $(INCLUDES_FOR_BUILD)
7+CFLAGS_FOR_BUILD = -DHOST_COMPILE -O2 -m2 -Wall -Werror -W $(INCLUDES_FOR_BUILD)
88 INCLUDES_FOR_BUILD = -I$(SOURCE_DIR)
99 LDFLAGS_FOR_BUILD = -nostartfiles $(START_UP_FOR_BUILD)
1010 LDLIBS_FOR_BUILD =
@@ -18,6 +18,7 @@
1818 sci_write.mot \
1919 sci_echoback.mot \
2020 timer_1sec.mot \
21+ timer_1sec.dis \
2122 motor_pwm_out.mot \
2223 motor_velocity_run.mot \
2324 encoder_print.mot \
@@ -39,8 +40,8 @@
3940
4041 sci_write.mot : sci_write.o $(BASE_OBJ) $(SOURCE_DIR)/sci_utilities.o $(SOURCE_DIR)/sci_read_write.o
4142 sci_echoback.mot : sci_echoback.o $(BASE_OBJ) $(SOURCE_DIR)/sci_read_write.o
42-timer_1sec.mot : timer_1sec.o $(BASE_OBJ) $(SOURCE_DIR)/timer_control.o itoa.o $(SOURCE_DIR)/std_string.o $(SOURCE_DIR)/sci_read_write.o
43-encoder_print.mot : encoder_print.o $(BASE_OBJ) $(SOURCE_DIR)/sci_utilities.o $(SOURCE_DIR)/encoder_reader.o $(SOURCE_DIR)/sci_read_write.o $(SOURCE_DIR)/mtu2_initialize.o
43+timer_1sec.mot timer_1sec.dis : timer_1sec.o $(BASE_OBJ) $(SOURCE_DIR)/timer_control.o itoa.o $(SOURCE_DIR)/std_string.o $(SOURCE_DIR)/sci_read_write.o
44+encoder_print.mot : encoder_print.o $(BASE_OBJ) $(SOURCE_DIR)/sci_utilities.o $(SOURCE_DIR)/encoder_reader.o $(SOURCE_DIR)/sci_read_write.o $(SOURCE_DIR)/mtu2_initialize.o $(SOURCE_DIR)/motor_pwm.o
4445 motor_pwm_out.mot : motor_pwm_out.o $(BASE_OBJ) $(SOURCE_DIR)/motor_pwm.o $(SOURCE_DIR)/mtu2_initialize.o
4546 motor_velocity_run.mot : motor_velocity_run.o $(BASE_OBJ) $(SOURCE_DIR)/timer_control.o $(SOURCE_DIR)/encoder_reader.o $(SOURCE_DIR)/motor_pwm.o $(SOURCE_DIR)/motor_velocity.o $(SOURCE_DIR)/mtu2_initialize.o
4647 pwm_output.mot : pwm_output.o $(SOURCE_DIR)/pwm_control.o
--- branches/tsukuba_2010/libs/run_drive/run_commands.c (nonexistent)
+++ branches/tsukuba_2010/libs/run_drive/run_commands.c (revision 1920)
@@ -0,0 +1,32 @@
1+/*!
2+ \file
3+ \brief 走行コマンドの処理
4+
5+ \author Satofumi KAMIMURA
6+
7+ $Id$
8+*/
9+
10+#include "run_commands.h"
11+#include "htoi.h"
12+#include "wheel_velocity.h"
13+
14+
15+void handle_WV_command(run_t *run, const char *line_buffer)
16+{
17+ int id;
18+ long velocity;
19+
20+ // パラメータの読み出し
21+ id = htoi(&line_buffer[2], 1);
22+ velocity = htoi(&line_buffer[3], 4);
23+
24+ // !!! パースに失敗したら、エラー応答を返すようにするべき
25+
26+ if ((id == 0) || (id == 1)) {
27+ wheel_set_velocity(&run->wheel[id], velocity);
28+ } else {
29+ return;
30+ }
31+ run->run_system.mode = DIRECT_WHEEL_CONTROL;
32+}
Added: svn:keywords
## -0,0 +1 ##
+Id
\ No newline at end of property
--- branches/tsukuba_2010/libs/run_drive/run_commands.h (nonexistent)
+++ branches/tsukuba_2010/libs/run_drive/run_commands.h (revision 1920)
@@ -0,0 +1,19 @@
1+#ifndef QRK_RUN_COMMANDS_H
2+#define QRK_RUN_COMMANDS_H
3+
4+/*!
5+ \file
6+ \brief 走行コマンドの処理
7+
8+ \author Satofumi KAMIMURA
9+
10+ $Id$
11+*/
12+
13+#include "run_t.h"
14+
15+
16+extern void handle_WV_command(run_t *run, const char *line_buffer);
17+// !!!
18+
19+#endif /* !QRK_RUN_COMMANDS_H */
Added: svn:keywords
## -0,0 +1 ##
+Id
\ No newline at end of property
旧リポジトリブラウザで表示