ROS2系列教程：控制硬件实战（舵机与传感器，原理）

1
PWM 信号 → 控制电路 → 比较角度误差 → 驱动电机 → 减速齿轮 → 输出轴
2
                    ↑_________________电位器反馈_______________|

1
0.5ms   →  0°（最小角度）
2
1.0ms   →  45°
3
1.5ms   →  90°（中位）
4
2.0ms   →  135°
5
2.5ms   →  180°（最大角度）

1
// servo.c：STM32 HAL 库控制舵机（TIM1 CH1，PE9）
2
#include "main.h"
3

4
// 初始化：定时器频率 1MHz（计数周期），ARR=19999 → 20ms 周期（50Hz）
5
void servo_init(void) {
6
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
7
    servo_set_angle(90);   // 先回中位
8
}
9

10
// 设置舵机角度：0~180 度 → 脉宽 0.5~2.5ms
11
void servo_set_angle(uint8_t angle) {
12
    if (angle > 180) angle = 180;
13
    // 脉宽(us) = 500 + angle * (2500-500)/180 = 500 + angle * 11.11
14
    // CCR 值 = 脉宽(us)（因为定时器是 1MHz，1 计数 = 1us）
15
    uint32_t pulse_us = 500 + (uint32_t)(angle * 11.11f);
16
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pulse_us);
17
}

1
// ESP32 Arduino：控制舵机（使用内置 Servo 库）
2
#include <ESP32Servo.h>
3

4
Servo myservo;
5

6
void setup() {
7
    myservo.attach(13, 500, 2500);  // GPIO13，脉宽 0.5~2.5ms
8
    myservo.write(90);              // 回中位
9
    delay(1000);
10
}
11

12
void loop() {
13
    // 0° → 180° → 0° 来回摆动
14
    for (int angle = 0; angle <= 180; angle += 2) {
15
        myservo.write(angle);
16
        delay(15);
17
    }
18
    delay(500);
19
    for (int angle = 180; angle >= 0; angle -= 2) {
20
        myservo.write(angle);
21
        delay(15);
22
    }
23
    delay(500);
24
}

1
总线舵机特点：
2
- 串口通信（UART），可级联 253 个
3
- 反馈角度、温度、电压（还能"感知"自己）
4
- 可设置速度、模式

1
角度 = 0.98 × (上一角度 + 陀螺仪角速度 × dt) + 0.02 × 加速度计角度

1
// mpu6050.c：读取加速度和陀螺仪原始数据（I2C）
2
#include "main.h"
3

4
#define MPU6050_ADDR  0xD0   // I2C 地址（AD0=0 时）
5
#define PWR_MGMT_1    0x6B
6
#define ACCEL_XOUT_H  0x3B
7
#define GYRO_XOUT_H   0x43
8

9
// 初始化：退出休眠模式
10
void mpu6050_init(void) {
11
    uint8_t data = 0x00;
12
    HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR, PWR_MGMT_1, 1, &data, 1, 100);
13
}
14

15
// 读三轴加速度 + 三轴陀螺仪
16
void mpu6050_read(int16_t *accel, int16_t *gyro) {
17
    uint8_t buf[14];
18
    HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR, ACCEL_XOUT_H, 1, buf, 14, 100);
19
    for (int i = 0; i < 6; i++) {
20
        int16_t v = (int16_t)((buf[i*2] << 8) | buf[i*2+1]);
21
        if (i < 3) accel[i] = v; else gyro[i-3] = v;
22
    }
23
}

1
# imu_publisher.py：把串口 IMU 数据发布为 ROS2 Imu 消息
2
import rclpy
3
import serial
4
import struct
5
from rclpy.node import Node
6
from sensor_msgs.msg import Imu
7
from std_msgs.msg import Header
8

9
class ImuPublisher(Node):
10
    def __init__(self):
11
        super().__init__('imu_publisher')
12
        self.pub = self.create_publisher(Imu, '/imu/data_raw', 10)
13
        self.ser = serial.Serial('/dev/ttyUSB1', 115200, timeout=0.1)
14
        self.timer = self.create_timer(0.02, self.tick)  # 50Hz
15

16
    def tick(self):
17
        # 读串口帧（简化：固定格式 6 个 int16）
18
        line = self.ser.readline()
19
        if len(line) < 12:
20
            return
21
        ax, ay, az, gx, gy, gz = struct.unpack('<6h', line[:12])
22

23
        msg = Imu()
24
        msg.header = Header()
25
        msg.header.stamp = self.get_clock().now().to_msg()
26
        msg.header.frame_id = 'base_link'
27
        # 加速度单位换算：原始值 / 16384 (±2g 量程)
28
        msg.linear_acceleration.x = ax / 16384.0 * 9.81
29
        msg.linear_acceleration.y = ay / 16384.0 * 9.81
30
        msg.linear_acceleration.z = az / 16384.0 * 9.81
31
        # 角速度：原始值 / 131 (±250°/s 量程)，转 rad/s
32
        msg.angular_velocity.x = gx / 131.0 * 3.14159 / 180.0
33
        msg.angular_velocity.y = gy / 131.0 * 3.14159 / 180.0
34
        msg.angular_velocity.z = gz / 131.0 * 3.14159 / 180.0
35
        self.pub.publish(msg)
36

37
def main(args=None):
38
    rclpy.init(args=args)
39
    node = ImuPublisher()
40
    rclpy.spin(node)
41
    node.destroy_node()
42
    rclpy.shutdown()
43

44
if __name__ == '__main__':
45
    main()

1
触发引脚（TRIG）：给 10us 高电平
2
回波引脚（ECHO）：输出高电平，宽度 = 往返时间
3
距离(cm) = 回波时间(us) / 58

1
// hc_sr04.c：STM32 读取距离（溢出捕获测回波脉宽）
2
#include "main.h"
3

4
float hc_sr04_read_distance_cm(void) {
5
    // 1. TRIG 给 10us 高电平触发
6
    HAL_GPIO_WritePin(TRIG_GPIO_Port, TRIG_Pin, GPIO_PIN_SET);
7
    delay_us(10);
8
    HAL_GPIO_WritePin(TRIG_GPIO_Port, TRIG_Pin, GPIO_PIN_RESET);
9

10
    // 2. 测量 ECHO 高电平时间（用定时器输入捕获）
11
    // 伪代码：启动捕获，等待上升沿开始计数，下降沿停止
12
    uint32_t pulse_us = measure_echo_pulse_us();  // 输入捕获实现
13

14
    // 3. 换算距离：1us ≈ 0.017cm（声速 340m/s）
15
    return pulse_us / 58.0f;
16
}

1
// 读取红外避障传感器：返回 1=有障碍，0=无障碍
2
uint8_t ir_obstacle_detect(void) {
3
    // 传感器输出接 GPIO（开漏，需要上拉）
4
    return HAL_GPIO_ReadPin(IR_GPIO_Port, IR_Pin) == GPIO_PIN_RESET ? 1 : 0;
5
}

1
header.frame_id: laser（坐标系）
2
angle_min / angle_max / angle_increment：角度范围与步进
3
range_min / range_max：有效距离范围
4
ranges[]：每个角度的距离值（米）

1
# detect.py：订阅摄像头图像做简单颜色检测（OpenCV）
2
import rclpy
3
from rclpy.node import Node
4
from sensor_msgs.msg import Image
5
from cv_bridge import CvBridge
6
import cv2
7
import numpy as np
8

9
class ColorDetect(Node):
10
    def __init__(self):
11
        super().__init__('color_detect')
12
        self.bridge = CvBridge()
13
        self.sub = self.create_subscription(
14
            Image, '/image_raw', self.on_image, 10)
15

16
    def on_image(self, msg):
17
        cv_img = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
18
        hsv = cv2.cvtColor(cv_img, cv2.COLOR_BGR2HSV)
19
        # 红色范围检测
20
        mask = cv2.inRange(hsv, (0, 100, 100), (10, 255, 255))
21
        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
22
        if contours:
23
            largest = max(contours, key=cv2.contourArea)
24
            x, y, w, h = cv2.boundingRect(largest)
25
            self.get_logger().info(f'red target at ({x},{y}) size {w}x{h}')
26

27
def main(args=None):
28
    rclpy.init(args=args)
29
    node = ColorDetect()
30
    rclpy.spin(node)
31
    rclpy.shutdown()
32

33
if __name__ == '__main__':
34
    main()

1
轮式里程计（短期准）+ IMU（姿态稳）+ 激光雷达（定位准）
2
    ↓ 融合算法（EKF / 粒子滤波）
3
机器人位姿估计（x, y, yaw）

1
// moving_average.c：滑动平均滤波（窗口 10）
2
#define WINDOW 10
3

4
float buf[WINDOW];
5
int idx = 0;
6
float sum = 0;
7
int filled = 0;
8

9
float moving_average(float sample) {
10
    if (filled == WINDOW) sum -= buf[idx];
11
    buf[idx] = sample;
12
    sum += sample;
13
    idx = (idx + 1) % WINDOW;
14
    filled = (filled < WINDOW) ? filled + 1 : WINDOW;
15
    return sum / filled;
16
}