控制阶段
舵机和超声波传感器结合实现测距和避障的功能
实现的基本思想：通过超声波传感器测量出小车到障碍物的距离，当距离小于一定值时，通过舵机转动带动超声波传感器一起转动，从而调整小车前进的方向（通过控制两个车轮的速度实现转向。）
部分代码：
#include <Servo.h> //舵机头文件
Servo myservo; //
设置舵机名
int val=35; //
舵机角度变量
int USValue = 0;//
定义存放超声波测量距离
int ledpin =11; //
定义灯作显示，小于30CM，灯亮
boolean flag=true;
int pos=0;//
设置角度标志，
//0
为30度，1为60度，2为90度，3为120度，4为150度
//
向超声波传感器发出的命令数组
byte DMcmd[4] = {0x22, 0x00, 0x00, 0x22};
//
超声波传感器返回的四个字节的变量
byte header;
byte highbyte;
byte lowbyte;
byte sum;
//
初始化
void setup()
{
Serial.begin(9600); //
串口波特率设置
pinMode(ledpin,OUTPUT); //
初始化，把灯熄灭
digitalWrite(ledpin,HIGH);
myservo.attach(9); //
连接舵机到数字接口9
myservo.write(val);
delay(200); //
等待舵机，到达转动位置
}
//
超声波测距子程序
void ultrasonic()
{
flag=true;
for(int i=0;i<4;i++)
{
Serial.print(DMcmd[i],BYTE);//
发送超声波测距命令
}
delay(50); //
一个延时
while(flag)
{
if(Serial.available()>0) //
查询串口有无数据
{
header=Serial.read(); //0x22
开始接收距离数据
highbyte=Serial.read();//
距离数据高8位
lowbyte=Serial.read();//
距离数据低8位
sum=Serial.read();//sum
校验和
if(highbyte==255) //
数据无效
{
USValue=65525; //
}
else
{
USValue =highbyte*256+lowbyte;
if (USValue<=30)
{//
如果距离小于30厘米小灯亮起
digitalWrite(ledpin,LOW);
}//
如果距离大于30厘米小灯熄灭
else
digitalWrite(ledpin,HIGH);
}
flag=false;
}
}
delay(20); //
延时
}
//
主程序
void loop()
{
for (int i=0; i <=3; i++)
//
舵机从30度，旋转5个角度，即30、60、90、120、150度
{
ultrasonic(); //
超声波测距
Serial.print(pos,BYTE);//
向上位机输出角度标志
Serial.print(highbyte,BYTE);//
向上位机输出被测距离
Serial.print(lowbyte,BYTE);//
pos+=1;//
角度标志加1
val+=30;//
舵机实际转角加30度
myservo.write(val); //
根据角度值，驱动舵机
delay(500); //
等待舵机，到达转动位置
}
for (int i=3; i >= 0; i--)
//
舵机从150度，旋转5个角度，即150、120、90、60、30度
{
ultrasonic(); //
超声波测距
Serial.print(pos,BYTE);//
向上位机输出角度标志
Serial.print(highbyte,BYTE);//
向上位机输出被测距离
Serial.print(lowbyte,BYTE);//
pos-=1;//
角度标志减1
val-=30;//
舵机实际转角减30度
myservo.write(val); //
根据角度值，驱动舵机
delay(500); //等待舵机，到达转动位置
}
}
