// Adafruit Motor shield library
// copyright Adafruit Industries LLC, 2009
// this code is public domain, enjoy!

#include <AFMotor.h>
#include <Servo.h>
//超声波
int TrgPin = A0;
int EcoPin = A1;

AF_DCMotor motor2(2);
AF_DCMotor motor3(3);
Servo servo;
//初始化距离数据
float dist;

void forward();
void backward();
void turnLeft();
void turnRight();
void stopCar();
void servoRight();
void servoLeft();
void servoForward();
int distance();
void servopulse(int servopin, int myangle);
void angle(int val);

int servopin = 9; //设置舵机驱动脚到数字口9
int myangle;//定义角度变量
int pulsewidth;//定义脉宽变量


void setup() {
  Serial.begin(9600);
  //设定舵机接口为输出接口
  pinMode(servopin, OUTPUT);
  //设置TrgPin为输出状态
  pinMode(TrgPin, OUTPUT);
  // 设置EcoPin为输入状态
  pinMode(EcoPin, INPUT);

  motor2.setSpeed(200);
  motor3.setSpeed(200);
}
void loop() {
  digitalWrite(TrgPin, LOW);
  delayMicroseconds(8);
  digitalWrite(TrgPin, HIGH);
  // 维持10毫秒高电平用来产生一个脉冲
  delayMicroseconds(10);
  digitalWrite(TrgPin, LOW);  // 读取脉冲的宽度并换算成距离
  dist = pulseIn(EcoPin, HIGH) / 58.00;
  Serial.print("Distance:");
  Serial.print(dist);
  Serial.println("cm");
  delay(300);

  if (dist< 35) //危险
  {
    stopCar();//电机停转
    servoLeft();//向左转舵机
    if (dist < 35) //危险
    {
      servoRight();//向右转舵机
      if (dist < 35) //危险
      {
        servoForward();//向中间舵机
        backward();            //后退
      }
      else
      {
        servoForward();//向中间舵机
        turnRight( );        //右转
        delay(30);
        stopCar();//电机停转
        forward();          //前进
      }
    }
    else
    {
      turnLeft( );        //左转
      delay(30);
      stopCar();//电机停转
      forward( );          //前进
    }
  }
  else
  {
    forward();          //前进
  }
}


//前进
void forward() {
  motor2.run(FORWARD);
  motor3.run(FORWARD);
}
//后退
void backward() {
  motor2.run(BACKWARD);
  motor3.run(BACKWARD);
}

//左转
void turnLeft() {
  motor2.run(FORWARD);
  motor3.run(RELEASE);
  delay(50);
}
//右转
void turnRight() {
  motor2.run(RELEASE);
  motor3.run(FORWARD);
  delay(50);
}
//停止
void stopCar() {
  motor2.run(RELEASE);
  motor3.run(RELEASE);
  delay(100);
}
//舵机右转
void servoRight() {
  servopulse(9, 20);
}
//舵机左转
void servoLeft() {
  servopulse(9, 180);
}
//舵机回正
void servoForward() {
  servopulse(9, 90);
}

//舵机角度的设定
void servopulse(int servopin, int myangle) /*定义一个脉冲函数，用来模拟方式产生PWM值*/
{
  for (int i = 0; i <= 50; i++) //产生PWM个数，等效延时以保证能转到响应角度
  {
    pulsewidth = (myangle * 11) + 500; //将角度转化为500-2480 的脉宽值
    digitalWrite(servopin, HIGH); //将舵机接口电平置高
    delayMicroseconds(pulsewidth);//延时脉宽值的微秒数
    digitalWrite(servopin, LOW); //将舵机接口电平置低
    delay(20 - pulsewidth / 1000); //延时周期内剩余时间
  }
}