• R/O
  • SSH
  • HTTPS

qrobosdk: コミット


コミットメタ情報

リビジョン1912 (tree)
日時2010-09-17 15:55:24
作者satofumi

ログメッセージ

motor_velocity.c までのサンプル動作を確認

変更サマリ

差分

--- branches/tsukuba_2010/libs/run_drive/motor_velocity.c (revision 1911)
+++ branches/tsukuba_2010/libs/run_drive/motor_velocity.c (revision 1912)
@@ -12,8 +12,8 @@
1212 #include "encoder_reader.h"
1313
1414 enum {
15- DEFAULT_GAIN_P = 5, /* 1/1 の重さ */
16- DEFAULT_GAIN_I = 1, /* 1/256 の重さ */
15+ DEFAULT_GAIN_P = 15, /* 1/1 の重さ */
16+ DEFAULT_GAIN_I = 16, /* 1/256 の重さ */
1717 };
1818
1919
@@ -42,6 +42,7 @@
4242 motor_pwm_direction_t direction;
4343
4444 // PI 制御
45+ total_diff = motor->total_diff;
4546 diff = target_count_velocity - actual_count_velocity;
4647 output = (motor->gain_p * diff) + (motor->gain_i * (total_diff >> 8));
4748 total_diff += diff;
@@ -56,8 +57,8 @@
5657 if (output >= 0) {
5758 direction = MOTOR_PWM_CW;
5859 } else {
60+ output = -output;
5961 direction = MOTOR_PWM_CCW;
60- output = -output;
6162 }
6263
6364 if (output > 256) {
--- branches/tsukuba_2010/libs/run_drive/Makefile (revision 1911)
+++ branches/tsukuba_2010/libs/run_drive/Makefile (revision 1912)
@@ -87,4 +87,5 @@
8787 timer_control.o: timer_control.h cpu_clock.h
8888 velocity_handler.o: velocity_handler.h
8989 wheel_velocity.o: wheel_velocity.h wheel_t.h encoder_t.h motor_t.h
90-wheel_velocity.o: encoder_reader.h motor_velocity.h motor_pwm.h
90+wheel_velocity.o: robot_parameter.h encoder_reader.h motor_velocity.h
91+wheel_velocity.o: motor_pwm.h
--- branches/tsukuba_2010/libs/run_drive/motor_pwm.c (revision 1911)
+++ branches/tsukuba_2010/libs/run_drive/motor_pwm.c (revision 1912)
@@ -123,30 +123,28 @@
123123 }
124124
125125
126-static void set_cw(int device_id, unsigned char duty)
126+static void set_ccw(int device_id, unsigned char duty)
127127 {
128- // PWM 出力が Lo のときに回転するので、指定された duty をここで反転させる
129128 if (device_id == 0) {
130- set_pe8(HI);
131- set_pe10_pwm(255 - duty);
129+ set_pe8(LO);
130+ set_pe10_pwm(duty);
132131
133132 } else if (device_id == 1) {
134- set_pe12(HI);
135- set_pe14_pwm(255 - duty);
133+ set_pe12(LO);
134+ set_pe14_pwm(duty);
136135 }
137136 }
138137
139138
140-static void set_ccw(int device_id, unsigned char duty)
139+static void set_cw(int device_id, unsigned char duty)
141140 {
142- // PWM 出力が Lo のときに回転するので、指定された duty をここで反転させる
143141 if (device_id == 0) {
144- set_pe10(HI);
145- set_pe8_pwm(255 - duty);
142+ set_pe10(LO);
143+ set_pe8_pwm(duty);
146144
147145 } else if (device_id == 1) {
148- set_pe14(HI);
149- set_pe12_pwm(255 - duty);
146+ set_pe14(LO);
147+ set_pe12_pwm(duty);
150148 }
151149 }
152150
--- branches/tsukuba_2010/libs/run_drive/wheel_t.h (revision 1911)
+++ branches/tsukuba_2010/libs/run_drive/wheel_t.h (revision 1912)
@@ -20,8 +20,8 @@
2020 encoder_t encoder;
2121 motor_t motor;
2222
23- int target_velocity; //!< [count/msec]
24- int next_add_velocity; //!< [count/msec]
23+ int target_velocity; //!< [mm/msec]
24+ int next_add_velocity; //!< [mm/msec]
2525 } wheel_t;
2626
2727 #endif /* !QRK_WHEEL_T_H */
--- branches/tsukuba_2010/libs/run_drive/encoder_reader.c (revision 1911)
+++ branches/tsukuba_2010/libs/run_drive/encoder_reader.c (revision 1912)
@@ -37,7 +37,7 @@
3737 }
3838
3939
40-static unsigned short encoder_count(int device_id)
40+static short encoder_count(int device_id)
4141 {
4242 if (device_id == 0) {
4343 return MTU21.TCNT;
@@ -64,7 +64,7 @@
6464
6565 void encoder_update(encoder_t *encoder)
6666 {
67- unsigned short current_count = encoder_count(encoder->device_id);
67+ short current_count = encoder_count(encoder->device_id);
6868
6969 encoder->difference = current_count - encoder->previous_count;
7070 encoder->previous_count = current_count;
--- branches/tsukuba_2010/libs/run_drive/wheel_velocity.c (revision 1911)
+++ branches/tsukuba_2010/libs/run_drive/wheel_velocity.c (revision 1912)
@@ -8,9 +8,11 @@
88 */
99
1010 #include "wheel_velocity.h"
11+#include "robot_parameter.h"
1112 #include "encoder_reader.h"
1213 #include "motor_velocity.h"
1314 #include "motor_pwm.h"
15+#include <math.h>
1416
1517
1618 void wheel_initialize(wheel_t *wheel, int device_id)
@@ -25,20 +27,37 @@
2527
2628 void wheel_set_velocity(wheel_t *wheel, int mm_per_sec)
2729 {
28- // 速度をカウント単位系に変換してから代入する
30+ wheel->target_velocity = mm_per_sec;
31+}
2932
30- // count_per_msec =
3133
32- (void)mm_per_sec;
33- wheel->target_velocity = -1;
34+static int mm2count_velocity(int mm)
35+{
36+ int count_per_msec =
37+ (mm * ENCODER_RESOLUTION) / (int)(2 * M_PI * WHEEL_RADIUS_MM) >> 10;
38+ return count_per_msec;
3439 }
3540
3641
42+static int count2mm_velocity(int count)
43+{
44+ int mm_per_sec =
45+ (count * (int)(2 * M_PI * WHEEL_RADIUS_MM) / ENCODER_RESOLUTION) << 10;
46+ return mm_per_sec;
47+}
48+
49+
3750 void wheel_velocity_control(wheel_t *wheel)
3851 {
39- // !!! 切り捨てた速度の補正
52+ int adjusted_mm_velocity =
53+ wheel->target_velocity + wheel->next_add_velocity;
54+ int count_velocity = mm2count_velocity(adjusted_mm_velocity);
4055
41- motor_velocity_control(&wheel->motor, wheel->target_velocity,
56+ // 整数の切捨て誤差を、次回の制御で加算されるようにする
57+ wheel->next_add_velocity =
58+ wheel->target_velocity - count2mm_velocity(count_velocity);
59+
60+ motor_velocity_control(&wheel->motor, count_velocity,
4261 encoder_difference(&wheel->encoder));
4362 }
4463
--- branches/tsukuba_2010/libs/run_drive/encoder_t.h (revision 1911)
+++ branches/tsukuba_2010/libs/run_drive/encoder_t.h (revision 1912)
@@ -15,7 +15,7 @@
1515 typedef struct
1616 {
1717 int device_id;
18- unsigned short previous_count;
18+ short previous_count;
1919 short difference;
2020 } encoder_t;
2121
--- branches/tsukuba_2010/libs/run_drive/samples/motor_velocity_run.c (revision 1911)
+++ branches/tsukuba_2010/libs/run_drive/samples/motor_velocity_run.c (revision 1912)
@@ -19,7 +19,7 @@
1919
2020
2121 enum {
22- TARGET_COUNT = 5,
22+ TARGET_COUNT = 1,
2323 };
2424
2525
@@ -29,17 +29,20 @@
2929 int current_count[2];
3030 int i;
3131
32+#if 1
33+ if (++msec >= 4) {
34+ msec = 0;
35+ } else {
36+ return;
37+ }
38+#endif
39+
3240 for (i = 0; i < 2; ++i) {
3341 encoder_update(&encoder_[i]);
3442 current_count[i] = encoder_difference(&encoder_[i]);
35- //motor_velocity_control(&motor_[i], -TARGET_COUNT, current_count[i]);
3643 }
37- motor_velocity_control(&motor_[0], -TARGET_COUNT, current_count[0]);
44+ motor_velocity_control(&motor_[0], +TARGET_COUNT, current_count[0]);
3845 motor_velocity_control(&motor_[1], +TARGET_COUNT, current_count[1]);
39-
40- if (++msec >= 1000) {
41- msec = 0;
42- }
4346 }
4447
4548
--- branches/tsukuba_2010/libs/run_drive/samples/wheel_velocity_run.c (revision 1911)
+++ branches/tsukuba_2010/libs/run_drive/samples/wheel_velocity_run.c (revision 1912)
@@ -42,12 +42,12 @@
4242 imask_initialize();
4343 clock_initialize();
4444 timer_initialize(INTERRUPT_PRIORITY_TIMER);
45+ set_imask_exr(0);
4546
4647 for (i = 0; i < 2; ++i) {
4748 wheel_initialize(&wheel_[i], i);
4849 wheel_set_velocity(&wheel_[i], VELOCITY_MM_PER_SEC);
4950 }
50- set_imask_exr(0);
5151
5252 timer_set_interval_function(timer_handler);
5353 timer_start();
--- branches/tsukuba_2010/libs/run_drive/samples/Makefile (revision 1911)
+++ branches/tsukuba_2010/libs/run_drive/samples/Makefile (revision 1912)
@@ -62,5 +62,7 @@
6262 timer_1sec.o: ../clock_initialize.h ../imask.h ../sci_read_write.h
6363 timer_1sec.o: ../timer_control.h ../interrupt_priority.h itoa.h
6464 timer_1sec.o: ../std_string.h
65-wheel_velocity_run.o: ../imask.h ../clock_initialize.h ../wheel_velocity.h
65+wheel_velocity_run.o: ../imask.h ../clock_initialize.h
66+wheel_velocity_run.o: ../interrupt_priority.h ../wheel_velocity.h
6667 wheel_velocity_run.o: ../wheel_t.h ../encoder_t.h ../motor_t.h
68+wheel_velocity_run.o: ../encoder_reader.h ../timer_control.h
--- branches/tsukuba_2010/libs/run_drive/motor_t.h (revision 1911)
+++ branches/tsukuba_2010/libs/run_drive/motor_t.h (revision 1912)
@@ -14,8 +14,8 @@
1414 //! モータ制御の管理
1515 typedef struct
1616 {
17- int gain_p;
18- int gain_i;
17+ long gain_p;
18+ long gain_i;
1919 int device_id;
2020 long total_diff;
2121 } motor_t;
--- branches/tsukuba_2010/libs/run_drive/robot_parameter.h (revision 1911)
+++ branches/tsukuba_2010/libs/run_drive/robot_parameter.h (revision 1912)
@@ -11,7 +11,7 @@
1111 */
1212
1313
14-#define ENCODER_RESOLUTION 100 //!< エンコーダ分解能 [1]
14+#define ENCODER_RESOLUTION 400 //!< エンコーダ分解能 [1]
1515 #define WHEEL_RADIUS_MM 43 //!< 車輪の半径 [mm]
1616 #define GEAR_RATIO 1.0 //!< ギヤ比 (モータ軸/車輪軸) [1]
1717
旧リポジトリブラウザで表示