#include <SoftwareSerial.h>
#include <Servo.h>
int echo = 2; //发射
int trig = 3; //接受
Servo myservo;//舵机对象
///////////////////////////////////////////////////
int distance()//测距函数distance
{
digitalWrite(trig,LOW);
delayMicroseconds(15);// 发出持续时间为 15us到 trigger脚驱动超声波检测
digitalWrite(trig,HIGH);
delayMicroseconds(15);
digitalWrite(trig,LOW);
int distance = pulseIn(echo,HIGH);// 接收脉冲的时间
// dist_mm =(time echo us * 0.34mm/us)/ 2 (mm)
distance= distance/58;//测得的距离转换为厘米
Serial.println("distance:"); 
Serial.println(distance,DEC);//输出距离值
delay(50);
return distance;
}
//////////////////////////////////////////////////////
int report(int angle_fun) { //报告速度，垂直距离以及是否报警
int dis_fun = 0; //障碍物的距离distance,单位厘米cm
int dis_minfun = 0; //障碍物与目标的垂直距离，单位厘米
int v_fun = 0; //障碍物移动的速度，单位厘米/秒
int dis1_fun = 0;
float pi = 3.141592;
int feng =4;
int buzzpin = 8; //led的引脚D8
//angle_fun=angle_fun+90;
myservo.write(angle_fun); // 指定舵机转向的角度
Serial.print("The angle is: "); //输出结果至串口监视器
Serial.print(angle_fun, DEC); //输出舵机的旋转角度
Serial.println("degree");  //与障碍的距离
dis_fun = distance(); //调用前面测距函数的值
Serial.print("dis_fun is: ");//进行计算速度
Serial.println(dis_fun); //输出速度值
delay(100);
dis1_fun = distance(); //将distance()函数来测出来的距离值赋予速度值
v_fun = (dis_fun - dis1_fun) * 10; //测出速度，速度厘米/秒
Serial.print("The rate is: "); //输出结果至串口监视器
Serial.print(v_fun, DEC);   //输出速度
Serial.println("cm/s");   //速度厘米/秒
dis_minfun = abs(sin(angle_fun * pi / 180) * dis1_fun);//用三角函数sin来计算垂直距离
Serial.print("The minimum of distance is: "); //输出结果至串口监视器
Serial.print(dis_minfun, DEC);  //输出垂直距离，单位为cm
Serial.println("cm");
//delay(1000);
{ if ((v_fun > 20) && (dis_minfun <= 100)) //如果速度大于20厘米/秒并且垂直距离小于100，报警
{
digitalWrite(buzzpin, HIGH);  //蜂鸣器为高电平
tone(4,262,250);//蜂鸣器以262的赫兹叫250ms
delay(4000);
return 1;
}
else
{ digitalWrite(buzzpin, LOW);  //蜂鸣器为低电平

return 0;
}
}
}
int angle = 0; //舵机的转动角度
int i = 5; //舵机每次步进转动的角度
int num;//报警提示
int feng = 4; //蜂鸣器的引脚D4
void setup() {  //初始化函数
Serial.begin(9600); //设置连接PC的串口波特率为 9600bps.
pinMode(echo,INPUT);  //发射口2号数字IO口为输入模式 
pinMode(trig,OUTPUT); // 接受口3号数字IO口为输出模式

pinMode(feng, OUTPUT);  //蜂鸣器4号数字IO口为输出模式
myservo.attach(11); // 舵机接arduinoD11脚
}
void loop() {  //循环函数
for (angle=0;angle <= 180;angle +=i){  //如果舵机的转动角度=0，小于等于180，或自加一个旋转角度
distance();  //输出距离值
digitalWrite(feng, HIGH); //蜂鸣器为高电平
report (angle);   //报出舵机的旋转角度
myservo.write(angle); 
delay(20);
digitalWrite(feng, LOW);  //蜂鸣器为高电平
}
for(angle=180;angle >= 0;angle -=i){ //如果舵机的转动角度等于180度，大于等于0度，或自减一个旋转角度
distance();     //输出距离值
report (angle);    //报告舵机的旋转角度
myservo.write(angle);
delay(20);
}
}