#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 clrtime=40;
int x=0;
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;
    }
    baseServo.write(x);             //调整舵机角度
    //测距
    distance = calculateDistance();
    //send distance
  }
  else if(status==1){
    if(x>0){
      x-=2;
    }
    else{
      status=2;
    }
    baseServo.write(x);             //调整舵机角度
    //测距
    distance = calculateDistance();
    //send distance
  }
  else if(status=2){
    clrtime--;
    if(clrtime<=0){
      clrtime=4;
      if(x>=180){
        status=1;
      }
      else{
        status=0;
      }
    }
    //send clear
  }
  delay(20-distance/17);
}