• R/O
  • SSH
  • HTTPS

qrobosdk: コミット


コミットメタ情報

リビジョン1924 (tree)
日時2010-09-23 19:59:04
作者satofumi

ログメッセージ

joystick にてロボットが動作

変更サマリ

差分

--- branches/tsukuba_2010/libs/run_master/Run_driver.cpp (revision 1923)
+++ branches/tsukuba_2010/libs/run_master/Run_driver.cpp (revision 1924)
@@ -8,7 +8,9 @@
88 */
99
1010 #include "Run_driver.h"
11+extern "C" {
1112 #include "c_run_driver.h"
13+}
1214 #include <string>
1315
1416 using namespace qrk;
@@ -45,8 +47,8 @@
4547
4648 bool Run_driver::open(const char *device)
4749 {
48- bool ret = run_driver_open(&pimpl->run_, device);
49- pimpl->error_message_ = run_driver_what();
50+ bool ret = (run_driver_open(&pimpl->run_, device) == 0) ? true : false;
51+ pimpl->error_message_ = run_driver_what(&pimpl->run_);
5052 return ret;
5153 }
5254
@@ -90,21 +92,21 @@
9092 }
9193
9294
93-bool Run_driver::set_path_previous_again(void)
95+long Run_driver::path_length_to_target(void) const
9496 {
9597 // !!!
96- return false;
98+ return -1;
9799 }
98100
99101
100-long Run_driver::path_length_to_target(void) const
102+bool Run_driver::stop_module(void)
101103 {
102104 // !!!
103- return -1;
105+ return false;
104106 }
105107
106108
107-bool Run_driver::set_pause_mode(void)
109+bool Run_driver::resume_module(void)
108110 {
109111 // !!!
110112 return false;
@@ -111,9 +113,13 @@
111113 }
112114
113115
114-void Run_driver::set_wheel_velocity(int wheel_id, long mm_per_sec)
116+bool Run_driver::set_wheel_velocity(int wheel_id, short mm_per_sec)
115117 {
116- (void)wheel_id;
117- (void)mm_per_sec;
118- // !!!
118+ if (run_driver_is_open(&pimpl->run_) != 0) {
119+ // !!! エラー状態を更新する
120+ return false;
121+ }
122+
123+ int ret = run_driver_set_wheel_velocity(&pimpl->run_, wheel_id, mm_per_sec);
124+ return (ret == 0) ? true : false;
119125 }
--- branches/tsukuba_2010/libs/run_master/c_run_driver.c (revision 1923)
+++ branches/tsukuba_2010/libs/run_master/c_run_driver.c (revision 1924)
@@ -8,31 +8,110 @@
88 */
99
1010 #include "c_run_driver.h"
11+#include <stdio.h>
1112
1213
13-bool run_driver_open(run_driver_t *run, const char *device)
14+enum {
15+ BAUDRATE = 38400,
16+ DEFAULT_TIMEOUT = 1000,
17+
18+ FALSE = 0,
19+ TRUE = 1,
20+};
21+
22+
23+static void initialize_run_driver_t(run_driver_t *run)
1424 {
15- // !!!
25+ sprintf(run->error_message, "no error.");
26+ run->is_opened = FALSE;
27+ run->timeout = DEFAULT_TIMEOUT;
1628 }
1729
1830
31+int run_driver_open(run_driver_t *run, const char *device)
32+{
33+ int ret;
34+
35+ initialize_run_driver_t(run);
36+
37+ ret = connection_open(&run->connection, URG_SERIAL, device, BAUDRATE);
38+ if (ret < 0) {
39+ // !!! エラーメッセージの更新
40+ (void)run->error_message;
41+
42+ } else {
43+ run->is_opened = TRUE;
44+ }
45+
46+ return run_driver_is_open(run);
47+}
48+
49+
50+int run_driver_is_open(run_driver_t *run)
51+{
52+ return (run->is_opened == TRUE) ? 0 : -1;
53+}
54+
55+
1956 void run_driver_close(run_driver_t *run)
2057 {
21- // !!!
58+ connection_close(&run->connection);
2259 }
2360
2461
25-const char *run_driver_what(void)
62+const char *run_driver_what(const run_driver_t *run)
2663 {
64+ return run->error_message;
65+}
66+
67+
68+void run_stop_module(void)
69+{
2770 // !!!
28- return "Not implemented.";
2971 }
3072
3173
32-bool run_driver_set_pause_state(run_driver_t *run)
74+void run_resume_module(void)
3375 {
34- (void)run;
3576 // !!!
77+}
3678
37- return false;
79+
80+int run_driver_set_wheel_velocity(run_driver_t *run,
81+ int wheel_id, short mm_per_sec)
82+{
83+ int n;
84+
85+ // !!! wheel_id と mm_per_sec の範囲判定を行うべき
86+
87+ if (run_driver_is_open(run) != 0) {
88+ // !!! エラーメッセージの更新
89+ return -1;
90+ }
91+
92+ // コマンドを送信
93+ enum {
94+ BUFFER_SIZE = 9,
95+ SEND_SIZE = 8,
96+ RECEIVE_SIZE = 4,
97+ };
98+ char buffer[BUFFER_SIZE] = "WVivvvv\n";
99+ snprintf(buffer, BUFFER_SIZE, "WV%d%04x\n", wheel_id, (unsigned short)mm_per_sec);
100+
101+ buffer[SEND_SIZE - 1] = '\n';
102+
103+ n = connection_write(&run->connection, buffer, SEND_SIZE);
104+ if (n != BUFFER_SIZE) {
105+ // !!! エラーメッセージの更新
106+ return -1;
107+ }
108+
109+ // 応答を受信
110+ n = connection_read(&run->connection, buffer, RECEIVE_SIZE, run->timeout);
111+ if (n != RECEIVE_SIZE) {
112+ // !!! エラーメッセージの更新
113+ return -1;
114+ }
115+
116+ return 0;
38117 }
--- branches/tsukuba_2010/libs/run_master/Run_driver.h (revision 1923)
+++ branches/tsukuba_2010/libs/run_master/Run_driver.h (revision 1924)
@@ -88,14 +88,6 @@
8888
8989
9090 /*!
91- \brief 経路コマンドの再発行
92-
93- !!! 文章でちゃんと説明する
94- */
95- bool set_path_previous_again(void);
96-
97-
98- /*!
9991 \brief 目標までの距離を取得
10092
10193 !!! 図で説明する
@@ -119,16 +111,20 @@
119111 // !!!
120112
121113
122- //! 走行モジュールを "位置停止" 状態に遷移させる
123- bool set_pause_mode(void);
114+ //! 走行モジュールを "緊急停止" させる
115+ bool stop_module(void);
124116
125117
118+ //! 走行モジュールの通常動作を再開する
119+ bool resume_module(void);
120+
121+
126122 // 車輪の速度を指示
127123 // !!! id の順番を定義する
128124 // !!! 0: 左
129125 // !!! 1: 右
130126 // !!! 戻り値を返すべきかを検討する
131- void set_wheel_velocity(int wheel_id, long mm_per_sec);
127+ bool set_wheel_velocity(int wheel_id, short mm_per_sec);
132128
133129
134130 private:
--- branches/tsukuba_2010/libs/run_master/c_run_driver.h (revision 1923)
+++ branches/tsukuba_2010/libs/run_master/c_run_driver.h (revision 1924)
@@ -13,12 +13,27 @@
1313 #include "run_driver_t.h"
1414
1515
16-extern bool run_driver_open(run_driver_t *run, const char *device);
16+extern int run_driver_open(run_driver_t *run, const char *device);
17+extern int run_driver_is_open(run_driver_t *run);
1718 extern void run_driver_close(run_driver_t *run);
18-extern const char *run_driver_what(void);
19+extern const char *run_driver_what(const run_driver_t *run);
1920
21+//qrk::Position<long> position() const;
22+
23+//bool set_path_line(const qrk::Position<long>& position,
24+//long stop_length);
25+
26+//bool set_path_circle(const qrk::Point<long>& center, long radius,
27+//Circle_rotation_direction direction,
28+//const Angle& stop_angle);
29+
30+//long path_length_to_target(void) const;
31+
2032 // !!!
2133
22-extern bool run_driver_set_pause_state(run_driver_t *run);
34+extern void run_stop_module(void);
35+extern void run_resume_module(void);
36+extern int run_driver_set_wheel_velocity(run_driver_t *run,
37+ int wheel_id, short mm_per_sec);
2338
2439 #endif /* !QRK_C_RUN_DRIVER_H */
--- branches/tsukuba_2010/libs/run_master/run_driver_t.h (revision 1923)
+++ branches/tsukuba_2010/libs/run_master/run_driver_t.h (revision 1924)
@@ -10,11 +10,21 @@
1010 $Id$
1111 */
1212
13+#include "urg_connection.h"
1314
15+
16+enum {
17+ RUN_DRIVER_T_BUFFER_SIZE = 128,
18+};
19+
20+
1421 //! 走行制御の管理
1522 typedef struct
1623 {
17- // !!!
24+ char error_message[RUN_DRIVER_T_BUFFER_SIZE];
25+ urg_connection_t connection;
26+ int is_opened;
27+ int timeout;
1828 } run_driver_t;
1929
2030 #endif /* !QRK_C_RUN_T_H */
--- branches/tsukuba_2010/libs/run_master/Makefile (revision 1923)
+++ branches/tsukuba_2010/libs/run_master/Makefile (revision 1924)
@@ -1,7 +1,7 @@
11 # taiyaki_master/
22
3-CC = $(CXX)
4-CXXFLAGS = -g -O0 -Wall -Werror -W $(INCLUDES)
3+CXXFLAGS = -g -O0 -Wall -Werror -W $(INCLUDES) `urg_c-config --cflags`
4+CFLAGS = $(CXXFLAGS)
55 INCLUDES = -I../geometry
66
77 RUN_MASTER_LIB = librun_master.a
--- branches/tsukuba_2010/libs/run_drive/setup_state.c (revision 1923)
+++ branches/tsukuba_2010/libs/run_drive/setup_state.c (revision 1924)
@@ -40,4 +40,6 @@
4040
4141 // 通信
4242 protocol_initialize();
43+
44+ set_imask_exr(INTERRUPT_PRIORITY_NO_MASK);
4345 }
--- branches/tsukuba_2010/libs/run_drive/emergency_io_control.c (revision 1923)
+++ branches/tsukuba_2010/libs/run_drive/emergency_io_control.c (revision 1924)
@@ -24,7 +24,7 @@
2424 static io_state_t stop_io_state(void)
2525 {
2626 // !!!
27- return IO_LOW;
27+ return IO_HIGH;
2828 }
2929
3030
@@ -31,7 +31,7 @@
3131 static io_state_t start_io_state(void)
3232 {
3333 // !!!
34- return IO_LOW;
34+ return IO_HIGH;
3535 }
3636
3737
--- branches/tsukuba_2010/libs/run_drive/control_state.c (revision 1923)
+++ branches/tsukuba_2010/libs/run_drive/control_state.c (revision 1924)
@@ -83,9 +83,12 @@
8383 timer_start();
8484
8585 while (true) {
86+#if 0
87+ // !!! 未配線のため、コメントアウト
8688 if (emergency_io_is_emergency()) {
8789 pause_state(run);
8890 }
91+#endif
8992
9093 // 通信による状態の更新
9194 protocol_update(run);
--- branches/tsukuba_2010/programs/run_wired_controller/run_wired_controller.cpp (revision 1923)
+++ branches/tsukuba_2010/programs/run_wired_controller/run_wired_controller.cpp (revision 1924)
@@ -34,7 +34,7 @@
3434 enum Velocity {
3535 Normal_velocity = 300, // [mm/sec]
3636 Slow_velocity = 100, // [mm/sec]
37- Rotate_velocity = 30, // [mm/sec]
37+ Rotate_velocity = 250, // [mm/sec]
3838 };
3939
4040 enum Buttons {
@@ -97,11 +97,11 @@
9797 void set_robot_velocity(Run_driver& robot_moving,
9898 long straight_velocity, long rotate_velocity)
9999 {
100- long left_wheel_velocity = straight_velocity - (rotate_velocity / 2);
101- long right_wheel_velocity = straight_velocity + (rotate_velocity / 2);
100+ long left_wheel_velocity = straight_velocity + (rotate_velocity / 2);
101+ long right_wheel_velocity = straight_velocity - (rotate_velocity / 2);
102102
103103 robot_moving.set_wheel_velocity(0, left_wheel_velocity);
104- robot_moving.set_wheel_velocity(1, right_wheel_velocity);
104+ robot_moving.set_wheel_velocity(1, -right_wheel_velocity);
105105 }
106106
107107
@@ -109,6 +109,14 @@
109109 {
110110 bool quit = false;
111111 while (!quit) {
112+ SDL_Event event;
113+ while (SDL_PollEvent(&event)) {
114+ joystick.set_event(&event);
115+ if (event.type == SDL_QUIT) {
116+ return;
117+ }
118+ }
119+
112120 // 操作情報の取得
113121 bool is_L1_pressed = joystick.is_button_pressed(L1_Button);
114122 bool is_L2_pressed = joystick.is_button_pressed(L2_Button);
--- branches/tsukuba_2010/programs/run_wired_controller/Makefile (revision 1923)
+++ branches/tsukuba_2010/programs/run_wired_controller/Makefile (revision 1924)
@@ -2,10 +2,10 @@
22
33 LIBS_DIR = ../../libs
44
5-CXXFLAGS = -g -O0 -Wall -Werror -W $(INCLUDES) `sdl-config --cflags`
5+CXXFLAGS = -g -O0 -Wall -Werror -W $(INCLUDES) `sdl-config --cflags` `urg_c-config --cflags`
66 INCLUDES = -I$(LIBS_DIR)/controller -I$(LIBS_DIR)/run_master -I$(LIBS_DIR)/geometry
77 LDFLAGS =
8-LDLIBS = `sdl-config --libs`
8+LDLIBS = `sdl-config --libs` `urg_c-config --libs`
99
1010 TARGET = run_wired_controller
1111
旧リポジトリブラウザで表示