超声波和舵机避障代码

### 使用超声波传感器和舵机实现避障功能 #### Arduino 实现方案 对于Arduino平台，可以采用HC-SR04超声波模块来检测障碍物的距离，并通过SG90微型舵机控制方向。下面是一个简单的代码示例： ```cpp #include <Servo.h> // 定义引脚编号 const int trigPin = 9; const int echoPin = 10; // 创建伺服对象 Servo myServo; void setup() { Serial.begin(9600); pinMode(trigPin, OUTPUT); // 设置触发信号为输出模式 pinMode(echoPin, INPUT); // 设置回响信号为输入模式 myServo.attach(7); // 将PWM信号连接到数字引脚7上 } void loop() { long duration; // 存储持续时间变量 float distance; // 计算距离值 digitalWrite(trigPin, LOW); delayMicroseconds(2); digitalWrite(trigPin, HIGH); delayMicroseconds(10); digitalWrite(trigPin, LOW); duration = pulseIn(echoPin, HIGH); distance = (duration / 2) * 0.0343; // 声音传播速度约为343m/s if(distance < 20){ // 当前方有物体且小于设定阈值时调整角度 turnRight(); }else{ goStraight(); } delay(500); // 每隔一段时间重新测量一次 } void turnRight(){ myServo.write(90); // 向右转动一定角度避开障碍物 delay(1000); // 维持该状态一秒以便完成转弯动作 } void goStraight(){ myServo.write(0); // 返回初始位置继续前行 } ``` 此段程序利用了`Servo`库管理舵机运动[^1]。 #### Raspberry Pi Pico 实现方案 针对Raspberry Pi Pico开发板，则推荐使用MicroPython编写类似的逻辑。这里给出一段适用于RP2040芯片的Python版本: ```python from machine import Pin, PWM import time # 初始化超声波测距所需GPIO针脚 trig = Pin(15, Pin.OUT) echo = Pin(14, Pin.IN) # 初始化舵机电压调节通道 servo_pwm = PWM(Pin(13)) servo_pwm.freq(50) def get_distance(): """获取当前环境中的最近障碍物距离""" trig.value(0) time.sleep_us(2) trig.value(1) time.sleep_us(10) trig.value(0) while not echo.value(): pass start_time = time.ticks_us() while echo.value(): pass end_time = time.ticks_us() elapsed_time = time.ticks_diff(end_time, start_time) distance_cm = ((elapsed_time / 2) * 0.0343) return round(distance_cm, 2) while True: dist = get_distance() print(f'Distance: {dist} cm') if dist < 20: servo_pwm.duty_u16(int((90 / 180) * 65535)) # 右转 time.sleep_ms(1000) else: servo_pwm.duty_u16(0) # 直行 time.sleep_ms(500) ``` 这段代码同样实现了基本的避障机制，在遇到近距离内的障碍物时会命令舵机改变方向以绕过它们。