mirs_esp
読み取り中…
検索中…
一致する文字列を見つけられません
topic_callback.ino
[詳解]
1//ノードのタイマーコールバック関数
2void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
3{
4 RCLC_UNUSED(last_call_time);
5 if (timer != NULL) {
6 //PID計算
8 //エンコーダーデータを格納
9 enc_msg.data.data[0] = count_l;
10 enc_msg.data.data[1] = count_r;
11 //電圧観測
12 vlt_watch();
13 // watchdog
14 if( (millis() - lastCalledAt) > WATCHDOG_TIMEOUT){
15 r_vel_cmd = 0;
16 l_vel_cmd = 0;
17 }
18
19 curr_vel_msg.data.data[0] = l_vel;
20 curr_vel_msg.data.data[1] = r_vel;
21 vlt_msg.data.data[0] = vlt_1;
22 vlt_msg.data.data[1] = vlt_2;
23 rcl_publish(&enc_pub, &enc_msg, NULL);
24 rcl_publish(&vlt_pub, &vlt_msg, NULL);
25 rcl_publish(&curr_vel_pub, &curr_vel_msg, NULL);
26 }
27}
28
29// cmd_velメッセージのコールバック関数
30void cmd_vel_Callback(const void * msgin) {
31 const geometry_msgs__msg__Twist * vel_msg = (const geometry_msgs__msg__Twist *)msgin;
32
33 // linear.x と angular.z のデータを取得
34 linear_x = vel_msg->linear.x;
35 angular_z = vel_msg->angular.z;
36
37 // 目標速度計算
40
41 // WatchDog用 最後に呼び出された時間を格納
42 lastCalledAt = millis();
43}
44
45//TODO: 消してサービスに移行
46//パラメーター更新のコールバック関数
47void param_Callback(const void * msgin){
48 const mirs_msgs__msg__BasicParam * param_msg = (const mirs_msgs__msg__BasicParam *)msgin;
49
50 // linear.x と angular.z のデータを取得
51 WHEEL_RADIUS = param_msg->wheel_radius;
52 WHEEL_BASE = param_msg->wheel_base;
53 RKP = param_msg->rkp;
54 RKI = param_msg->rki;
55 RKD = param_msg->rkd;
56 LKP = param_msg->lkp;
57 LKI = param_msg->lki;
58 LKD = param_msg->lkd;
59}
double RKP
double LKI
double RKI
double LKP
double RKD
double LKD
double WHEEL_RADIUS
double WHEEL_BASE
#define WATCHDOG_TIMEOUT
uint32_t lastCalledAt
rcl_publisher_t vlt_pub
rcl_publisher_t enc_pub
double l_vel
double vlt_2
std_msgs__msg__Float64MultiArray curr_vel_msg
float linear_x
mirs_msgs__msg__BasicParam param_msg
double r_vel
double l_vel_cmd
double r_vel_cmd
std_msgs__msg__Float64MultiArray vlt_msg
std_msgs__msg__Int32MultiArray enc_msg
int32_t count_r
double vlt_1
int32_t count_l
rcl_timer_t timer
float angular_z
rcl_publisher_t curr_vel_pub
void param_Callback(const void *msgin)
void timer_callback(rcl_timer_t *timer, int64_t last_call_time)
void cmd_vel_Callback(const void *msgin)
void PID_control()
void vlt_watch()