第九章 学完后运行没问题，我加入显示屏显示、时间和电压测试功能后小车不受控制到处跑，请教下该如何处理？
-
这是第九章完成测试没有问题的代码（下文称为第一代码块）：
#include <Arduino.h> #include <Esp32PcntEncoder.h> #include <Esp32McpwmMotor.h> // 导入电机驱动库 #include <PidController.h> // 包含 PID 控制器库，用于实现 PID 控制 #include <Kinematics.h> // 包含运动学库，用于计算运动轨迹 #include <micro_ros_platformio.h> // 包含用于 ESP32 的 micro-ROS PlatformIO 库 #include <WiFi.h> // 包含 ESP32 的 WiFi 库 #include <rcl/rcl.h> // 包含 ROS 客户端库 (RCL) #include <rclc/rclc.h> // 包含用于 C 的 ROS 客户端库 (RCLC) #include <rclc/executor.h> // 包含 RCLC 执行程序库，用于执行订阅和发布消息 #include <geometry_msgs/msg/twist.h> // 包含 ROS 消息库，用于发布/订阅 Twist 消息 #include <nav_msgs/msg/odometry.h> #include <micro_ros_utilities/string_utilities.h> Esp32McpwmMotor motor; Esp32PcntEncoder encoders[2]; // 创建一个数组用于存储两个编码器 PidController pid_controller[2]; // 创建PidController的两个对象，用于控制两个电机的速度 Kinematics kinematics; // 创建一个Kinematics对象，用于计算运动轨迹 // float target_linear_speed = 50.0; // 设置目标直线速度为50mm/s // float target_angular_speed = 0.1f; // 设置目标角速度为0.1rad/s float out_left_speed; // 用于存储左电机速度 float out_right_speed; // 用于存储右电机速度 rclc_executor_t executor; rclc_support_t support; rcl_allocator_t allocator; rcl_node_t node; rcl_subscription_t subscriber; // 定义订阅句柄 geometry_msgs__msg__Twist sub_msg; // 定义接收到的消息结构体 rcl_publisher_t odom_publisher; // 用于发布机器人的里程计信息（Odom） nav_msgs__msg__Odometry odom_msg; // 机器人的里程计信息 rcl_timer_t timer; // 定义计时器句柄 void callback_publisher(rcl_timer_t * timer, int64_t last_call_time) { odom_t odom = kinematics.get_odom(); // 获取机器人的里程计信息 int64_t stamp = rmw_uros_epoch_millis(); // 获取当前时间戳 odom_msg.header.stamp.sec = static_cast<uint32_t>(stamp / 1000); // 设置时间戳的秒部分 odom_msg.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 设置时间戳的纳秒部分 odom_msg.pose.pose.position.x = odom.x; // 设置机器人的x坐标 odom_msg.pose.pose.position.y = odom.y; // 设置机器人的y坐标 odom_msg.pose.pose.orientation.w = cos(odom.angle * 0.5); // 设置机器人的朝向 odom_msg.pose.pose.orientation.x = 0; // 设置机器人的朝向的x分量 odom_msg.pose.pose.orientation.y = 0; // 设置机器人的朝向的y分量 odom_msg.pose.pose.orientation.z = sin(odom.angle * 0.5); // 设置机器人的朝向的z分量 odom_msg.twist.twist.angular.z = odom.angle_speed; // 设置机器人的角速度 odom_msg.twist.twist.linear.x = odom.linear_speed; // 设置机器人的线速度 if(rcl_publish(&odom_publisher, &odom_msg, NULL)!=RCL_RET_OK) { // 发布机器人的里程计信息 Serial.printf("error: odom publisher failed!\n"); } } // 回调函数，当接收到新的 Twist 消息时会被调用 void twist_callback(const void *msg_in) { // 将接收到的消息指针转化为 geometry_msgs__msg__Twist 类型 const geometry_msgs__msg__Twist *twist_msg = (const geometry_msgs__msg__Twist *)msg_in; kinematics.kinematic_inverse(twist_msg->linear.x*1000, twist_msg->angular.z, out_left_speed, out_right_speed); // 计算运动轨迹 pid_controller[0].update_target(out_left_speed); // 设置第一个电机的目标速度 pid_controller[1].update_target(out_right_speed); // 设置第二个电机的目标速度 } void micro_ros_task(void *parameter) { IPAddress agent_ip; agent_ip.fromString("192.168.43.228"); // 设置代理IP地址 set_microros_wifi_transports("HONOR 9X", "123456789", agent_ip, 8888); // 设置WiFi传输和代理IP地址和端口 delay(2000); allocator = rcl_get_default_allocator(); rclc_support_init(&support, 0, NULL, &allocator); // 初始化ROS支持 rclc_node_init_default(&node, "fishbot_motion_control", "", &support); // 初始化ROS节点 unsigned int num_handles = 0+2; // 计算需要添加的句柄数量 rclc_executor_init(&executor, &support.context, num_handles, &allocator); // 初始化执行程序 rclc_subscription_init_best_effort(&subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist), "/cmd_vel"); // 初始化订阅句柄，并设置消息类型和主题名称 rclc_executor_add_subscription(&executor, &subscriber, &sub_msg, &twist_callback, ON_NEW_DATA); // 添加订阅句柄 odom_msg.header.frame_id = micro_ros_string_utilities_set(odom_msg.header.frame_id, "odom"); // 设置里程计信息中的frame_id odom_msg.child_frame_id = micro_ros_string_utilities_set(odom_msg.child_frame_id, "base_footprint"); // 设置里程计信息中的child_frame_id rclc_publisher_init_best_effort(&odom_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry), "/odom"); // 初始化里程计信息发布器 while (!rmw_uros_epoch_synchronized()) { rmw_uros_sync_session(1000); // 等待ROS网络同步 delay(10); // 等待10毫秒 } rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(50), callback_publisher); // 初始化计时器 rclc_executor_add_timer(&executor, &timer); // 将计时器添加到执行程序中 rclc_executor_spin(&executor); // 开始执行程序 } void setup() { // 1.初始化串口 Serial.begin(115200); // 初始化串口通信，设置通信速率为115200 // 2.设置编码器 encoders[0].init(0, 32, 33); // 初始化第一个编码器，使用GPIO 32和33连接 encoders[1].init(1, 26, 25); // 初始化第二个编码器，使用GPIO 26和25连接 motor.attachMotor(0, 22, 23); // 连接电机到MCPWM 0通道，使用GPIO 12和13连接 motor.attachMotor(1, 12, 13); // 连接电机到MCPWM 1通道，使用GPIO 27和36连接 // 3.设置PID控制器 pid_controller[0].update_pid(0.625, 0.125, 0.00); // 设置第一个电机的PID参数 pid_controller[1].update_pid(0.625, 0.125, 0.00); // 设置第二个电机的PID参数 pid_controller[0].out_limit(-100, 100); // 设置第一个电机的输出限幅 pid_controller[1].out_limit(-100, 100); // 设置第二个电机的输出限幅 kinematics.set_wheel_distance(175); // 设置轮距为175mm，即轮子之间的距离 kinematics.set_motor_param(0, 0.1051566); // 设置第一个电机的每脉冲距离 kinematics.set_motor_param(1, 0.1051566); // 设置第二个电机的每脉冲距离 // kinematics.kinematic_inverse(target_linear_speed, target_angular_speed, out_left_speed, out_right_speed); // 根据目标数度计算左右轮的速度 // pid_controller[0].update_target(out_left_speed); // 设置第一个电机的目标速度 // pid_controller[1].update_target(out_right_speed); // 设置第二个电机的目标速度 xTaskCreate(micro_ros_task, "micro_ros", 10240, NULL, 1, NULL); // 创建micro-ROS任务 } void loop() { delay(10); // 等待10毫秒 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("x=%f, y=%f, angle=%f\n", kinematics.get_odom().x, kinematics.get_odom().y, kinematics.get_odom().angle); // 打印当前位置信息 }然后我看显示屏没有点亮，我就在这个代码的基础上加入了显示屏显示、电压测量、WiFi显示和时间显示4个功能，编译和下传都没有问题，导航时就开始随意无规律乱跑。以下是完整代码：
#include <Arduino.h> #include <Esp32PcntEncoder.h> #include <Esp32McpwmMotor.h> // 导入电机驱动库 #include <PidController.h> // 包含 PID 控制器库，用于实现 PID 控制 #include <Kinematics.h> // 包含运动学库，用于计算运动轨迹 #include <micro_ros_platformio.h> // 包含用于 ESP32 的 micro-ROS PlatformIO 库 #include <WiFi.h> // 包含 ESP32 的 WiFi 库 #include <rcl/rcl.h> // 包含 ROS 客户端库 (RCL) #include <rclc/rclc.h> // 包含用于 C 的 ROS 客户端库 (RCLC) #include <rclc/executor.h> // 包含 RCLC 执行程序库，用于执行订阅和发布消息 #include <geometry_msgs/msg/twist.h> // 包含 ROS 消息库，用于发布/订阅 Twist 消息 #include <nav_msgs/msg/odometry.h> #include <micro_ros_utilities/string_utilities.h> #include <TimeLib.h> // 加载时间库，提供setTime\year\month...函数 #include <Wire.h> // 加载Wire库,用于I2C通信 #include <Adafruit_GFX.h> // 加载Adafruit_GFX库 #include <Adafruit_SSD1306.h> // 加载Adafruit_SSD1306库 Adafruit_SSD1306 display(128, 64, &Wire); //创建OLED显示屏对象，使用Wire总线 const int timeout_ms = 1000; // 设置同步时间时的全局变量4个参数分别是：同步时间，同步周期，同步偏移量，同步时间源。 static int64_t time_ms; static time_t time_seconds; char time_str[25]; Esp32McpwmMotor motor; Esp32PcntEncoder encoders[2]; // 创建一个数组用于存储两个编码器 PidController pid_controller[2]; // 创建PidController的两个对象，用于控制两个电机的速度 Kinematics kinematics; // 创建一个Kinematics对象，用于计算运动轨迹 float out_left_speed; // 用于存储左电机速度 float out_right_speed; // 用于存储右电机速度 rclc_executor_t executor; rclc_support_t support; rcl_allocator_t allocator; rcl_node_t node; rcl_subscription_t subscriber; // 定义订阅句柄 geometry_msgs__msg__Twist sub_msg; // 定义接收到的消息结构体 rcl_publisher_t odom_publisher; // 用于发布机器人的里程计信息（Odom） nav_msgs__msg__Odometry odom_msg; // 机器人的里程计信息 rcl_timer_t timer; // 定义计时器句柄 void callback_publisher(rcl_timer_t * timer, int64_t last_call_time) { odom_t odom = kinematics.get_odom(); // 获取机器人的里程计信息 int64_t stamp = rmw_uros_epoch_millis(); // 获取当前时间戳 odom_msg.header.stamp.sec = static_cast<uint32_t>(stamp / 1000); // 设置时间戳的秒部分 odom_msg.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 设置时间戳的纳秒部分 odom_msg.pose.pose.position.x = odom.x; // 设置机器人的x坐标 odom_msg.pose.pose.position.y = odom.y; // 设置机器人的y坐标 odom_msg.pose.pose.orientation.w = cos(odom.angle * 0.5); // 设置机器人的朝向 odom_msg.pose.pose.orientation.x = 0; // 设置机器人的朝向的x分量 odom_msg.pose.pose.orientation.y = 0; // 设置机器人的朝向的y分量 odom_msg.pose.pose.orientation.z = sin(odom.angle * 0.5); // 设置机器人的朝向的z分量 odom_msg.twist.twist.angular.z = odom.angle_speed; // 设置机器人的角速度 odom_msg.twist.twist.linear.x = odom.linear_speed; // 设置机器人的线速度 if(rcl_publish(&odom_publisher, &odom_msg, NULL)!=RCL_RET_OK) { // 发布机器人的里程计信息 Serial.printf("error: odom publisher failed!\n"); } } // 回调函数，当接收到新的 Twist 消息时会被调用 void twist_callback(const void *msg_in) { // 将接收到的消息指针转化为 geometry_msgs__msg__Twist 类型 const geometry_msgs__msg__Twist *twist_msg = (const geometry_msgs__msg__Twist *)msg_in; kinematics.kinematic_inverse(twist_msg->linear.x*1000, twist_msg->angular.z, out_left_speed, out_right_speed); // 计算运动轨迹 pid_controller[0].update_target(out_left_speed); // 设置第一个电机的目标速度 pid_controller[1].update_target(out_right_speed); // 设置第二个电机的目标速度 } void micro_ros_task(void *parameter) { IPAddress agent_ip; agent_ip.fromString("192.168.43.228"); // 设置代理IP地址 set_microros_wifi_transports("HONOR 9X", "123456789", agent_ip, 8888); // 设置WiFi传输和代理IP地址和端口 delay(2000); allocator = rcl_get_default_allocator(); rclc_support_init(&support, 0, NULL, &allocator); // 初始化ROS支持 rclc_node_init_default(&node, "fishbot_motion_control", "", &support); // 初始化ROS节点 unsigned int num_handles = 0+2; // 计算需要添加的句柄数量 rclc_executor_init(&executor, &support.context, num_handles, &allocator); // 初始化执行程序 rclc_subscription_init_best_effort(&subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist), "/cmd_vel"); // 初始化订阅句柄，并设置消息类型和主题名称 rclc_executor_add_subscription(&executor, &subscriber, &sub_msg, &twist_callback, ON_NEW_DATA); // 添加订阅句柄 odom_msg.header.frame_id = micro_ros_string_utilities_set(odom_msg.header.frame_id, "odom"); // 设置里程计信息中的frame_id odom_msg.child_frame_id = micro_ros_string_utilities_set(odom_msg.child_frame_id, "base_footprint"); // 设置里程计信息中的child_frame_id rclc_publisher_init_best_effort(&odom_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry), "/odom"); // 初始化里程计信息发布器 while (!rmw_uros_epoch_synchronized()) { rmw_uros_sync_session(1000); // 等待ROS网络同步 delay(10); // 等待10毫秒 } rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(50), callback_publisher); // 初始化计时器 rclc_executor_add_timer(&executor, &timer); // 将计时器添加到执行程序中 rclc_executor_spin(&executor); // 开始执行程序 } void setup() { // 1.初始化串口 Serial.begin(115200); // 初始化串口通信，设置通信速率为115200 Wire.begin(18, 19); // 初始化I2C通信 display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // 初始化OLED显示屏，使用GPIO 18和19连接 display.clearDisplay(); // 清空屏幕 display.setTextSize(1); // 设置字体大小 display.setCursor(0, 0); // 设置开始显示文字的坐标 display.setTextColor(SSD1306_WHITE); // 设置字体颜色 display.println("hello code001!"); // 输出的字符 display.display(); // 显示字符 pinMode(34, INPUT); // 设置测量电池电压引脚模式为输入 analogSetAttenuation(ADC_11db); // 设置ADC的衰减为11db encoders[0].init(0, 32, 33); // 初始化第一个编码器，使用GPIO 32和33连接 encoders[1].init(1, 26, 25); // 初始化第二个编码器，使用GPIO 26和25连接 motor.attachMotor(0, 22, 23); // 连接电机到MCPWM 0通道，使用GPIO 12和13连接 motor.attachMotor(1, 12, 13); // 连接电机到MCPWM 1通道，使用GPIO 27和36连接 // 3.设置PID控制器 pid_controller[0].update_pid(0.625, 0.125, 0.00); // 设置第一个电机的PID参数 pid_controller[1].update_pid(0.625, 0.125, 0.00); // 设置第二个电机的PID参数 pid_controller[0].out_limit(-100, 100); // 设置第一个电机的输出限幅 pid_controller[1].out_limit(-100, 100); // 设置第二个电机的输出限幅 kinematics.set_wheel_distance(175); // 设置轮距为175mm，即轮子之间的距离 kinematics.set_motor_param(0, 0.1051566); // 设置第一个电机的每脉冲距离 kinematics.set_motor_param(1, 0.1051566); // 设置第二个电机的每脉冲距离 xTaskCreate(micro_ros_task, "micro_ros", 10240, NULL, 1, NULL); // 创建micro-ROS任务 } void loop() { 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("x=%f, y=%f, angle=%f\n", kinematics.get_odom().x, kinematics.get_odom().y, kinematics.get_odom().angle); // 打印当前位置信息 display.clearDisplay(); // 清空屏幕 if (WiFi.status() == WL_CONNECTED) { display.setCursor(0, 0); display.print("WiFi: "); display.println(WiFi.SSID()); // 显示WiFi名称 } else { display.setCursor(0, 0); display.println("Connection failed"); // 显示WiFi未连接状态 } int analogValue = analogRead(34); // 读取原始值0-4096 int analogVolts = analogReadMilliVolts(34); // 读取模拟电压，单位毫伏 float realVolts = 5.02 * ((float)analogVolts * 1e-3); // 计算实际电压值 Serial.printf("realVolts value = %.2f\n", realVolts); // 串口输出实际电压值 display.setCursor(0, 16); // 设置开始显示文字的坐标 display.print("realVolts value:"); display.printf("%.2f\n", realVolts); // 显示实际电压值 display.setCursor(0, 32); display.print("123456789012345678901"); // 输出任意英文和数字字符的 while (!rmw_uros_epoch_synchronized()) // 判断时间是否同步,如果没有同步就等待时间同步，直到同步为止。启动bringup.launch.py后，等待一段时间，直到时间同步。 { rmw_uros_sync_session(timeout_ms); // 同步时间 if (rmw_uros_epoch_synchronized()) { time_ms = rmw_uros_epoch_millis(); // 获取当前时间 time_seconds = time_ms / 1000; setTime(time_seconds + 8 * 3600); // 将当前时间+8H到北京时间然后设置到系统 } return; } /*sprintf 函数的作用就是将格式化的数据写入到time_str变量中*/ sprintf(time_str, "%04d-%02d-%02d %02d:%02d:%02d ", year(), month(), day(), hour(), minute(), second()); display.setCursor(0, 48); // 设置开始显示文字的坐标 display.println(time_str); // 输出的字符 display.display(); // delay(1000); // }经过2天的测试，发现问题很奇怪：
1、首先认为导航问题，我把导航关闭后用键盘控制，一样乱跑；
2、其次测试时间代码部分可能因为共用WiFi接口冲突，注释掉所有显示时间的功能，结果还是乱跑；
3、接着在注释掉电压测量部分继续乱跑；
4、然后注释掉显示屏显示功能依然乱跑；
5、最后将已改动的功能包上把所有的变化删除还原到第一个代码块，还是乱跑。如果用完全没有经历过上述代码修改的第一个代码块测试，任然没有问题。
我想请教下小鱼老师我该怎么啦？实在是测试不出来了，还望小鱼老师空闲时指导一二。 -
@onedream delay1000 去掉把，休眠了会影响程序正常运行的，你参考下fishbot固件的源码，里面有一起运行的实现的方法。
-
@小鱼 感谢小鱼老师，太神奇了，我把所有的都测试了就是没有删除delay（1000），因为我看到课本上也有个delay（10），永远也没想到那里去。结果把delay一删就恢复正常了，完全没有问题。顺便我问下你提到的固件源码哪里下载啦？专门的小车界面都是根据配置助手直接下载到小车的.bin文件，我没有找到源码。
-
-
此回复已被删除！ -
@小鱼 收到，小鱼老师，感谢！感谢！我继续去学习。
