舵机旋转找出可以通行的方向

目的： 如果测距传感器前面有阻挡物，则左右旋转舵机，探测出左右距离，然后输出距离较大的一边。

 

舵机接线：

一根是红色，连到+5V上。一根棕色（有些是黑的），连到GND。还有一根是黄色或者橘色，连到数字引脚9。

超声波传感器接线：

VCC    -- 供5V电源

TRIG   -- 触发控制信号输入

ECHO -- 回响信号输出等四个接口端

GND   -- 为地线

 

#define Trig 2 //引脚Tring 连接 IO D2
#define Echo 3 //引脚Echo 连接 IO D3 
float cm; //距离变量
float temp; 

#include <Servo.h> // 声明调用Servo.h库
Servo myservo; // 创建一个舵机对象
void setup() 
{
  myservo.attach(9);  // 舵机信号线连接D9
  pinMode(Trig, OUTPUT); 
  pinMode(Echo, INPUT);
  myservo.write(90); //通电时，舵机朝向为90方向
  Serial.begin(9600);
  }
void loop() 
{ 

  int jvli = distance();
  if(jvli<10)  //如果距离小于10厘米，舵机开始旋转
  {
    myservo.write(150);  
    delay(1000); 
    int a = distance();
    Serial.println(a); //输出左侧距离
    delay(100);
    myservo.write(30); 
    delay(1000); 
    int b = distance();
    Serial.println(b); //输出右侧距离
    delay(100);
    if(a<=b)
    {
      Serial.println("右侧可以通行");
      }else
      {
        Serial.println("左侧可以通行");
        }
    myservo.write(90);
    delay(100);
    }

  }

int distance()
 {
  //给Trig发送一个低高低的短时间脉冲,触发测距
  digitalWrite(Trig, LOW); //给Trig发送一个低电平
  delayMicroseconds(2);    //等待 2微妙
  digitalWrite(Trig,HIGH); //给Trig发送一个高电平
  delayMicroseconds(10);    //等待 10微妙
  digitalWrite(Trig, LOW); //给Trig发送一个低电平
  temp = float(pulseIn(Echo, HIGH)); //存储回波等待时间,
  cm = (temp * 17 )/1000; //把回波时间换算成cm
  return cm;
  delay (100);
  }