mirs_esp
読み取り中…
検索中…
一致する文字列を見つけられません
vel_ctrl.ino
[詳解]
2 //エンコーダーの変化量を計算
3 int32_t delta_left = count_l - prev_count_l;
4 int32_t delta_right = count_r - prev_count_r;
5
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
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
77 // ledcのPWM設定
78 pinMode(PIN_DIR_R, OUTPUT);
79 pinMode(PIN_DIR_L, OUTPUT);
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}
#define PIN_DIR_R
定義 config.h:8
double RKP
double LKI
#define COUNTS_PER_REV
double RKI
double LKP
double RKD
const int32_t r_Channel
double LKD
const int32_t pwmResolution
#define PIN_PWM_R
定義 config.h:9
#define PIN_PWM_L
double WHEEL_RADIUS
const int32_t pwmFrequency
const int32_t l_Channel
#define PIN_DIR_L
float prev_r_err
double l_vel
std_msgs__msg__Float64MultiArray curr_vel_msg
int32_t prev_count_r
float l_err_sum
float r_err_sum
double r_vel
double l_vel_cmd
double r_vel_cmd
float prev_l_err
std_msgs__msg__Int32MultiArray enc_msg
int32_t count_r
int32_t prev_count_l
int32_t count_l
void vel_ctrl_set()
void PID_control()
void calculate_vel()