@1257261547 89793abb-de57-486a-836b-49d3d0d37589-图片.png
code_text
main.cpp
#include <Arduino.h>
#include <Esp32PcntEncoder.h>
#include <Esp32McpwmMotor.h>
#include <PidController.h>
#include <Kinematics.h>
#include <Warning.h>
#include <ImuDriver.h>
#include <WiFi.h>
#include <micro_ros_platformio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <geometry_msgs/msg/twist.h>
#include <nav_msgs/msg/odometry.h>
#include <micro_ros_utilities/string_utilities.h>
#include <sensor_msgs/msg/imu.h>
rcl_allocator_t allocator;
rclc_support_t support;
rclc_executor_t executor;
rcl_node_t node;
rcl_subscription_t sub_cmd_vel;
geometry_msgs__msg__Twist msg_cmd_vel;
rcl_publisher_t pub_odom;
nav_msgs__msg__Odometry msg_odom;
rcl_timer_t timer;
rcl_publisher_t pub_imu;
sensor_msgs__msg__Imu msg_imu;
Esp32PcntEncoder encoders[2]; // 创建一个数组用于存储两个编码器
Esp32McpwmMotor motor; // 创建一个名为motor的对象，用于控制电机
PidController pid_controller[2];
Kinematics kinematics;
MPU6050 mpu(Wire); // 初始化MPU6050对象
ImuDriver imu(mpu); // 初始化Imu对象
imu_t imu_data; // IMU 数据对象
float target_linear_speed = 20.0; // 单位 毫米每秒
float target_angular_speed = 0.1; // 单位 弧度每秒
float out_left_speed = 0.0; // 输出的左右轮速度，不是反馈的左右轮速度
float out_right_speed = 0.0;
void start()
{
pinMode(TRIG, OUTPUT);
pinMode(ECHO, INPUT);
pinMode(WARN, OUTPUT);
}
void timer_callback(rcl_timer_t* timer,int64_t last_call_time)
{
odom_t odom = kinematics.get_odom();
int64_t stamp = rmw_uros_epoch_millis();
msg_odom.header.stamp.sec = static_cast<int32_t>(stamp/1000);
msg_odom.header.stamp.nanosec = static_cast<int32_t>((stamp%1000)*1e6);
msg_odom.pose.pose.position.x = odom.x;
msg_odom.pose.pose.position.y = odom.y;
msg_odom.pose.pose.orientation.w = cos(odom.angle*0.5);
msg_odom.pose.pose.orientation.x = 0;
msg_odom.pose.pose.orientation.y = 0;
msg_odom.pose.pose.orientation.z = sin(odom.angle*0.5);
msg_odom.twist.twist.linear.x = odom.linear_speed;
msg_odom.twist.twist.angular.z = odom.angular_speed;
imu.getImuDriverData(imu_data);
msg_imu.header.stamp.sec = static_cast<int32_t>(stamp / 1000); // 秒部分
msg_imu.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 纳秒部分
msg_imu.angular_velocity.x = imu_data.angular_velocity.x;
msg_imu.angular_velocity.y = imu_data.angular_velocity.y;
msg_imu.angular_velocity.z = imu_data.angular_velocity.z;
msg_imu.linear_acceleration.x = imu_data.linear_acceleration.x;
msg_imu.linear_acceleration.y = imu_data.linear_acceleration.y;
msg_imu.linear_acceleration.z = imu_data.linear_acceleration.z;
msg_imu.orientation.x = imu_data.orientation.x;
msg_imu.orientation.y = imu_data.orientation.y;
msg_imu.orientation.z = imu_data.orientation.z;
msg_imu.orientation.w = imu_data.orientation.w;
if(rcl_publish(&pub_odom,&msg_odom,NULL)!=RCL_RET_OK)
{
Serial.printf("error: odom pub failed!");
}
if(rcl_publish(&pub_imu, &msg_imu, NULL)!=RCL_RET_OK)
{
Serial.printf("error: imu pub failed!");
}
}
void twist_callback(const void * msg_in)
{
const geometry_msgs__msg__Twist* msg = (const geometry_msgs__msg__Twist*)msg_in;
target_linear_speed = msg->linear.x*1000;
target_angular_speed = msg->angular.z;
kinematics.kinematics_inverse(target_linear_speed, target_angular_speed, &out_left_speed, &out_right_speed);
Serial.printf("OUT:left_speed=%f,right_speed=%f\n", out_left_speed, out_right_speed);
pid_controller[0].update_target(out_left_speed);
pid_controller[1].update_target(out_right_speed);
}
void microros_task(void* args)
{
IPAddress agent_ip;
agent_ip.fromString("192.168.188.93");
set_microros_wifi_transports("xbw_mi","Xbw159357",agent_ip,8888);
delay(2000);
allocator = rcl_get_default_allocator();
rclc_support_init(&support,0,NULL,&allocator);
rclc_node_init_default(&node,"example_xbwbot_motion_control","",&support);
unsigned int num_handles = 3;
rclc_executor_init(&executor,&support.context,num_handles,&allocator);
rclc_subscription_init_best_effort(&sub_cmd_vel,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs,msg,Twist),"/cmd_vel");
rclc_executor_add_subscription(&executor,&sub_cmd_vel,&msg_cmd_vel,&twist_callback,ON_NEW_DATA);
msg_odom.header.frame_id = micro_ros_string_utilities_set(msg_odom.header.frame_id,"odom");
msg_odom.child_frame_id = micro_ros_string_utilities_set(msg_odom.child_frame_id,"base_footprint");
msg_imu.header.frame_id = micro_ros_string_utilities_set(msg_imu.header.frame_id,"imu");
rclc_publisher_init_best_effort(&pub_odom,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs,msg,Odometry),"/odom");
rclc_publisher_init_best_effort(&pub_imu,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu),"/imu");
rclc_timer_init_default(&timer,&support,RCL_MS_TO_NS(50),timer_callback);
rclc_executor_add_timer(&executor,&timer);
while(!rmw_uros_epoch_synchronized())
{
rmw_uros_sync_session(1000);
delay(10);
}
rclc_executor_spin(&executor);
}
// v=wr r = v/w
void setup()
{
// 初始化串口
Serial.begin(115200); // 初始化串口通信，设置通信速率为115200
// 初始化电机驱动器2
encoders[0].init(0, 32, 33); // 初始化第一个编码器，使用GPIO 32和33连接
encoders[1].init(1, 26, 25); // 初始化第二个编码器，使用GPIO 26和25连接
// 初始化PID控制器的参数
pid_controller[0].update_pid(0.625, 0.125, 0.0);
pid_controller[1].update_pid(0.625, 0.125, 0.0);
pid_controller[0].out_limit(-100, 100);
pid_controller[1].out_limit(-100, 100);
// 初始化运动学参数
kinematics.set_wheel_distance(175); // mm
kinematics.set_motor_param(0, 0.103657);
kinematics.set_motor_param(1, 0.103657);
// 测试下运动学逆解
xTaskCreate(microros_task,"microros_task",10240,NULL,1,NULL);
start();
imu.begin(18, 19);
}
void loop()
{
digitalWrite(TRIG, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG, LOW);
double delta_time = pulseIn(ECHO, HIGH);
float detect_distance = delta_time * 0.0343 / 2;
if(detect_distance<50)
{
digitalWrite(WARN,HIGH);
}
else
{
digitalWrite(WARN,LOW);
}
delayMicroseconds(9990);
kinematics.update_motor_speed(millis(), encoders[0].getTicks(), encoders[1].getTicks());
motor.updateMotorSpeed(0, pid_controller[0].update(kinematics.get_motor_speed(0)));
motor.updateMotorSpeed(1, pid_controller[1].update(kinematics.get_motor_speed(1)));
// Serial.printf("speed1=%d,speed2=%d\n",kinematics.get_motor_speed(0),kinematics.get_motor_speed(1));
// Serial.printf("x,y,yaw=%f,%f,%f\n",kinematics.get_odom().x,kinematics.get_odom().y,kinematics.get_odom().angle);
imu.update();
}
我使用的main.cpp，PidController.h和PidController.h与第九章的代码相同，ImuDriver.h和.cpp是从https://github.dev/fishros/fishbot_motion_control_microros复制过来的。
