mirs_esp
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
vel_ctrl.ino
[詳解]
1
void
calculate_vel
(){
2
//エンコーダーの変化量を計算
3
int32_t delta_left =
count_l
-
prev_count_l
;
4
int32_t delta_right =
count_r
-
prev_count_r
;
5
6
prev_count_l
=
count_l
;
7
prev_count_r
=
count_r
;
8
9
// 車輪の回転角度(ラジアン)を計算
10
double
delta_left_rad = (delta_left /
COUNTS_PER_REV
) * 2.0 * PI;
11
double
delta_right_rad = (delta_right /
COUNTS_PER_REV
) * 2.0 * PI;
12
13
// それぞれの車輪の移動距離を計算
14
double
left_distance = delta_left_rad *
WHEEL_RADIUS
;
15
double
right_distance = delta_right_rad *
WHEEL_RADIUS
;
16
17
// 現在の速度を計算
18
l_vel
= left_distance / 0.1;
19
r_vel
= right_distance / 0.1;
20
}
21
22
void
PID_control
(){
23
calculate_vel
();
24
25
float
r_err =
r_vel_cmd
-
r_vel
;
26
float
l_err =
l_vel_cmd
-
l_vel
;
27
28
r_err_sum
+= r_err;
29
l_err_sum
+= l_err;
30
31
// PID計算を実行
32
double
r_pwm =
RKP
* r_err +
RKI
*
r_err_sum
+
RKD
* (r_err -
prev_r_err
);
33
double
l_pwm =
LKP
* l_err +
LKI
*
l_err_sum
+
LKD
* (l_err -
prev_l_err
);
34
35
prev_r_err
= r_err;
36
prev_l_err
= l_err;
37
38
if
(r_pwm > 255){
39
r_pwm = 255;
40
}
else
if
(r_pwm < -255){
41
r_pwm = -255;
42
}
43
if
(l_pwm > 255){
44
l_pwm = 255;
45
}
else
if
(l_pwm < -255){
46
l_pwm = -255;
47
}
48
49
// 出力値の前処理
50
if
(r_pwm >= 0){
51
digitalWrite(
PIN_DIR_R
, LOW);
52
}
else
{
53
r_pwm *= -1;
54
digitalWrite(
PIN_DIR_R
,HIGH);
55
}
56
if
(l_pwm >= 0){
57
digitalWrite(
PIN_DIR_L
, HIGH);
58
}
else
{
59
l_pwm *= -1;
60
digitalWrite(
PIN_DIR_L
,LOW);
61
}
62
63
// 確実な停止
64
if
(
r_vel_cmd
== 0){
65
r_pwm = 0;
66
}
67
if
(
l_vel_cmd
== 0){
68
l_pwm = 0;
69
}
70
71
// pwm出力
72
ledcWrite(
r_Channel
, uint8_t(r_pwm));
73
ledcWrite(
l_Channel
, uint8_t(l_pwm));
74
}
75
76
void
vel_ctrl_set
() {
77
// ledcのPWM設定
78
pinMode(
PIN_DIR_R
, OUTPUT);
79
pinMode(
PIN_DIR_L
, OUTPUT);
80
ledcSetup(
r_Channel
,
pwmFrequency
,
pwmResolution
);
81
ledcSetup(
l_Channel
,
pwmFrequency
,
pwmResolution
);
82
ledcAttachPin(
PIN_PWM_R
,
r_Channel
);
83
ledcAttachPin(
PIN_PWM_L
,
l_Channel
);
84
85
curr_vel_msg
.data.size = 2;
// メッセージ配列のサイズを2に設定
86
curr_vel_msg
.data.data = (
double
*)malloc(
enc_msg
.data.size *
sizeof
(
double
));
// 配列のメモリを確保
87
curr_vel_msg
.data.data[0] = 0;
88
curr_vel_msg
.data.data[1] = 0;
89
}
PIN_DIR_R
#define PIN_DIR_R
定義
config.h:8
RKP
double RKP
定義
config.h:22
LKI
double LKI
定義
config.h:26
COUNTS_PER_REV
#define COUNTS_PER_REV
定義
config.h:30
RKI
double RKI
定義
config.h:23
LKP
double LKP
定義
config.h:25
RKD
double RKD
定義
config.h:24
r_Channel
const int32_t r_Channel
定義
config.h:35
LKD
double LKD
定義
config.h:27
pwmResolution
const int32_t pwmResolution
定義
config.h:38
PIN_PWM_R
#define PIN_PWM_R
定義
config.h:9
PIN_PWM_L
#define PIN_PWM_L
定義
config.h:11
WHEEL_RADIUS
double WHEEL_RADIUS
定義
config.h:31
pwmFrequency
const int32_t pwmFrequency
定義
config.h:37
l_Channel
const int32_t l_Channel
定義
config.h:36
PIN_DIR_L
#define PIN_DIR_L
定義
config.h:10
prev_r_err
float prev_r_err
定義
mirs26_esp.ino:71
l_vel
double l_vel
定義
mirs26_esp.ino:63
curr_vel_msg
std_msgs__msg__Float64MultiArray curr_vel_msg
定義
mirs26_esp.ino:24
prev_count_r
int32_t prev_count_r
定義
mirs26_esp.ino:57
l_err_sum
float l_err_sum
定義
mirs26_esp.ino:69
r_err_sum
float r_err_sum
定義
mirs26_esp.ino:68
r_vel
double r_vel
定義
mirs26_esp.ino:62
l_vel_cmd
double l_vel_cmd
定義
mirs26_esp.ino:61
r_vel_cmd
double r_vel_cmd
定義
mirs26_esp.ino:60
prev_l_err
float prev_l_err
定義
mirs26_esp.ino:72
enc_msg
std_msgs__msg__Int32MultiArray enc_msg
定義
mirs26_esp.ino:22
count_r
int32_t count_r
定義
mirs26_esp.ino:54
prev_count_l
int32_t prev_count_l
定義
mirs26_esp.ino:56
count_l
int32_t count_l
定義
mirs26_esp.ino:53
vel_ctrl_set
void vel_ctrl_set()
定義
vel_ctrl.ino:76
PID_control
void PID_control()
定義
vel_ctrl.ino:22
calculate_vel
void calculate_vel()
定義
vel_ctrl.ino:1
mirs26_esp
vel_ctrl.ino
構築:
1.18.0