77 lines
1.4 KiB
C++
77 lines
1.4 KiB
C++
#include <Servo.h>
|
|
|
|
#define trigPin 6 //超声波模块的Trig口 6#
|
|
#define echoPin 5 //超声波模块的echo口 5#
|
|
#define ServoPin 3 //底座舵机端口 3#
|
|
|
|
unsigned long time=0;
|
|
Servo baseServo;
|
|
bool increasing=true;
|
|
bool clr=false;
|
|
//0: increase 1: decrease 2: clr
|
|
int status=0;
|
|
int clrtimes=40;
|
|
void setup(void)
|
|
{
|
|
pinMode(trigPin, OUTPUT); //设置trigPin端口模式
|
|
pinMode(echoPin, INPUT); //设置echoPin端口模式
|
|
Serial.begin(9600); //设置串口传输率
|
|
baseServo.attach(ServoPin); //初始化舵机
|
|
baseServo.write(0);
|
|
|
|
}
|
|
int calculateDistance()
|
|
{
|
|
long duration;
|
|
digitalWrite(trigPin, LOW);
|
|
delayMicroseconds(2);
|
|
digitalWrite(trigPin, HIGH);
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigPin, LOW);
|
|
duration = pulseIn(echoPin, HIGH);
|
|
return duration*0.034/2;
|
|
}
|
|
|
|
void loop(void)
|
|
{
|
|
int distance=0;
|
|
if(status==0){
|
|
if(x<180){
|
|
x+=2;
|
|
}
|
|
else{
|
|
status=2;
|
|
}
|
|
}
|
|
else if(status==1){
|
|
if(x>0){
|
|
x-=2;
|
|
}
|
|
else{
|
|
status=2;
|
|
}
|
|
}
|
|
else if(status=2){
|
|
|
|
clrtime--;
|
|
if(clrtime<=0){
|
|
clrtime=4;
|
|
if(x>=180){
|
|
status=1;
|
|
}
|
|
else{
|
|
status=0;
|
|
}
|
|
}
|
|
}
|
|
if(status!=2){
|
|
baseServo.write(x); //调整舵机角度
|
|
//测距
|
|
distance = calculateDistance();
|
|
//send distance
|
|
}
|
|
else{
|
|
//send clear
|
|
}
|
|
delay(20-distance/17);
|
|
} |