• R/O
  • SSH
  • HTTPS

qrobosdk: コミット


コミットメタ情報

リビジョン1913 (tree)
日時2010-09-17 16:38:06
作者satofumi

ログメッセージ

車輪の速度制御を確認

変更サマリ

差分

--- branches/tsukuba_2010/libs/run_drive/motor_velocity.c (revision 1912)
+++ branches/tsukuba_2010/libs/run_drive/motor_velocity.c (revision 1913)
@@ -12,8 +12,8 @@
1212 #include "encoder_reader.h"
1313
1414 enum {
15- DEFAULT_GAIN_P = 15, /* 1/1 の重さ */
16- DEFAULT_GAIN_I = 16, /* 1/256 の重さ */
15+ DEFAULT_GAIN_P = 16, /* 1/1 の重さ */
16+ DEFAULT_GAIN_I = 32, /* 1/256 の重さ */
1717 };
1818
1919
@@ -42,7 +42,6 @@
4242 motor_pwm_direction_t direction;
4343
4444 // PI 制御
45- total_diff = motor->total_diff;
4645 diff = target_count_velocity - actual_count_velocity;
4746 output = (motor->gain_p * diff) + (motor->gain_i * (total_diff >> 8));
4847 total_diff += diff;
--- branches/tsukuba_2010/libs/run_drive/motor_pwm.c (revision 1912)
+++ branches/tsukuba_2010/libs/run_drive/motor_pwm.c (revision 1913)
@@ -41,6 +41,7 @@
4141 #include "mtu2_initialize.h"
4242 #include <7125S.H>
4343
44+
4445 typedef enum {
4546 LO = 0,
4647 HI = 1,
@@ -123,28 +124,28 @@
123124 }
124125
125126
126-static void set_ccw(int device_id, unsigned char duty)
127+static void set_cw(int device_id, unsigned char duty)
127128 {
128129 if (device_id == 0) {
129- set_pe8(LO);
130- set_pe10_pwm(duty);
130+ set_pe8(HI);
131+ set_pe10_pwm(255 - duty);
131132
132133 } else if (device_id == 1) {
133- set_pe12(LO);
134- set_pe14_pwm(duty);
134+ set_pe12(HI);
135+ set_pe14_pwm(255 - duty);
135136 }
136137 }
137138
138139
139-static void set_cw(int device_id, unsigned char duty)
140+static void set_ccw(int device_id, unsigned char duty)
140141 {
141142 if (device_id == 0) {
142- set_pe10(LO);
143- set_pe8_pwm(duty);
143+ set_pe10(HI);
144+ set_pe8_pwm(255 - duty);
144145
145146 } else if (device_id == 1) {
146- set_pe14(LO);
147- set_pe12_pwm(duty);
147+ set_pe14(HI);
148+ set_pe12_pwm(255 - duty);
148149 }
149150 }
150151
--- branches/tsukuba_2010/libs/run_drive/wheel_velocity.c (revision 1912)
+++ branches/tsukuba_2010/libs/run_drive/wheel_velocity.c (revision 1913)
@@ -34,7 +34,8 @@
3434 static int mm2count_velocity(int mm)
3535 {
3636 int count_per_msec =
37- (mm * ENCODER_RESOLUTION) / (int)(2 * M_PI * WHEEL_RADIUS_MM) >> 10;
37+ mm * ENCODER_RESOLUTION / (int)(2 * M_PI * WHEEL_RADIUS_MM) / 1000;
38+
3839 return count_per_msec;
3940 }
4041
@@ -42,7 +43,8 @@
4243 static int count2mm_velocity(int count)
4344 {
4445 int mm_per_sec =
45- (count * (int)(2 * M_PI * WHEEL_RADIUS_MM) / ENCODER_RESOLUTION) << 10;
46+ count * (int)(2 * M_PI * WHEEL_RADIUS_MM) / ENCODER_RESOLUTION * 1000;
47+
4648 return mm_per_sec;
4749 }
4850
@@ -55,7 +57,7 @@
5557
5658 // 整数の切捨て誤差を、次回の制御で加算されるようにする
5759 wheel->next_add_velocity =
58- wheel->target_velocity - count2mm_velocity(count_velocity);
60+ adjusted_mm_velocity - count2mm_velocity(count_velocity);
5961
6062 motor_velocity_control(&wheel->motor, count_velocity,
6163 encoder_difference(&wheel->encoder));
旧リポジトリブラウザで表示