//传感器程序加舵机，实现避障
//舵机头文件
#include <LobotServoController.h>
LobotServoController myse;
void setup() {
  //蜂鸣器设定模式，对应3号引脚
  pinMode(3,OUTPUT);
  //超声波传感器设定模式，对应8，9号引脚
  pinMode(8,OUTPUT);//Tring输出
  pinMode(9,INPUT);//Echo输入

  //舵机控制板设定模式，对应13号引脚
  pinMode(13,OUTPUT);
  Serial.begin(9600);
  while(!Serial);
  digitalWrite(13,HIGH);
}

void loop() {
  //控制超声波发射脉冲
  digitalWrite(8,HIGH);
  delay(1);
    digitalWrite(8,LOW);
  //然后读取一个值，用long这个变量读取时间
  long t=pulseIn(9,HIGH);///////////////////这一条十分重要
  float d=t*0.034/2;
//蜂鸣器工作
if (d>30){
  digitalWrite(3,LOW);
  }
  else if(d<=30&&d>20){
  digitalWrite(3,HIGH);
  delay(500);
  digitalWrite(3,LOW);
  delay(500);
  }
  else if(d<=20&&d>15){
  digitalWrite(3,HIGH);
  delay(100);
  digitalWrite(3,LOW);
  delay(100);
  }
  else{
  digitalWrite(3,HIGH);
  }
//舵机工作
if(d>30){
  //舵机直线运动
  myse.runActionGroup(1,5);  //运行1号动作组 5次
  delay(3000);
}
else if (d>=15&&d<=30){
  //舵机执行固定避障操作
  //调用舵机控制板的动作组
myse.runActionGroup(2,2);  //运行2号动作组 2次
  delay(3000);
myse.runActionGroup(1,5);  //运行1号动作组 5次
  delay(3000);
myse.runActionGroup(3,2);  //运行3号动作组 2次
  delay(3000);

}
else if(d<=15){
  //舵机停止运动
  myse.runActionGroup(4,1);  //运行4号动作组 1次
  delay(3000);

}

}