mirs_esp
読み取り中…
検索中…
一致する文字列を見つけられません
mirs26_esp.ino
[詳解]
1#include <micro_ros_arduino.h>
2
3#include <micro_ros_arduino.h>
4
5#include <micro_ros_arduino.h>
6#include <stdio.h>
7#include <rcl/rcl.h>
8#include <rcl/error_handling.h>
9#include <rclc/rclc.h>
10#include <rclc/executor.h>
11#include <std_msgs/msg/int32_multi_array.h>
12#include <geometry_msgs/msg/twist.h>
13#include <mirs_msgs/srv/parameter_update.h>
14#include <mirs_msgs/srv/simple_command.h>
15#include <mirs_msgs/msg/basic_param.h>
16#include <std_msgs/msg/float64_multi_array.h>
17#include <mirs_msgs/action/trigger.h>
18#include <pthread.h>
19#include "config.h"
20
21//topic通信で使用するメッセージ宣言
22std_msgs__msg__Int32MultiArray enc_msg; //エンコーダー情報
23std_msgs__msg__Float64MultiArray vlt_msg; //電圧情報
24std_msgs__msg__Float64MultiArray curr_vel_msg; //速度情報
25geometry_msgs__msg__Twist cmd_vel_msg; //速度指令値
26mirs_msgs__msg__BasicParam param_msg; //パラメーターメッセージ
27
28//service通信で使用するメッセージ宣言
29mirs_msgs__srv__ParameterUpdate_Response update_res;
30mirs_msgs__srv__ParameterUpdate_Request update_req;
31mirs_msgs__srv__SimpleCommand_Response reset_res;
32mirs_msgs__srv__SimpleCommand_Request reset_req;
33
34//publisher,subscriber,serviceの宣言
35rcl_publisher_t enc_pub;
36rcl_publisher_t vlt_pub;
37rcl_publisher_t curr_vel_pub;
38rcl_subscription_t cmd_vel_sub;
39rcl_subscription_t param_sub;
40rcl_service_t update_srv;
41rcl_service_t reset_srv;
42
43//ノードに関わる宣言
44rclc_executor_t executor;
45rclc_support_t support;
46rcl_allocator_t allocator;
47rcl_node_t node;
48rcl_timer_t timer;
49
50/* 処理で使用するグローバル変数 */
51
52//エンコーダーカウント
53int32_t count_l = 0;
54int32_t count_r = 0;
55
56int32_t prev_count_l = 0;
57int32_t prev_count_r = 0;
58
59//速度制御用の変数
60double r_vel_cmd;
61double l_vel_cmd;
62double r_vel;
63double l_vel;
64
65float linear_x; // 直進速度
66float angular_z; // 回転速度
67
68float r_err_sum = 0;
69float l_err_sum = 0;
70
71float prev_r_err = 0;
72float prev_l_err = 0;
73
74//電圧
75double vlt_1 = 0;
76double vlt_2 = 0;
77
78//WatchDog用
79uint32_t lastCalledAt;
80
81void setup() {
82 ros_setup();
83
86 vlt_setup();
87
88 delay(500);
89}
90
91void loop() {
92 delay(10);
93 rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
94}
機体のピン、通信、制御に使う定数を定義する。
void encoder_open()
float prev_r_err
rcl_service_t reset_srv
mirs_msgs__srv__SimpleCommand_Response reset_res
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
void setup()
rcl_subscription_t param_sub
int32_t prev_count_r
float l_err_sum
mirs_msgs__srv__SimpleCommand_Request reset_req
float linear_x
mirs_msgs__srv__ParameterUpdate_Response update_res
rcl_service_t update_srv
rclc_executor_t executor
float r_err_sum
mirs_msgs__msg__BasicParam param_msg
double r_vel
geometry_msgs__msg__Twist cmd_vel_msg
rcl_subscription_t cmd_vel_sub
double l_vel_cmd
double r_vel_cmd
mirs_msgs__srv__ParameterUpdate_Request update_req
rcl_node_t node
float prev_l_err
std_msgs__msg__Float64MultiArray vlt_msg
std_msgs__msg__Int32MultiArray enc_msg
rcl_allocator_t allocator
int32_t count_r
double vlt_1
int32_t prev_count_l
rclc_support_t support
int32_t count_l
rcl_timer_t timer
float angular_z
void loop()
rcl_publisher_t curr_vel_pub
void ros_setup()
void vel_ctrl_set()
void vlt_setup()