超声雷达

【舵机的颜色】舵机引出三根线。一根是红色的，连+5V，一根是黑色或者棕色的，连GND，第三根可能是白色、黄色或者橘色，连数字引脚。

 

线的颜色	连接
红色	+5V
黑色或棕色	GND
白色、黄色或橘色	信号线

 

#include<Servo.h>
const int soundTriggerPin = 2;
const int soundEchoPin = 3;
const int motorSignalPin = 7;
const int startingAngle = 15;
const int minimumAngle = 15;
const int maximumAngle = 165;
const int rotationSpeed = 1;
Servo motor;　　//这个库不使用引脚9、10的pwm功能
void setup(void) 
{
    pinMode(soundTriggerPin, OUTPUT);
    pinMode(soundEchoPin, INPUT);
    motor.attach(motorSignalPin); 　　　　//设置舵机接口
    Serial.begin(9600);
}
void loop(void)
{
    static int motorAngle = startingAngle;
    static int motorRotateAmount = rotationSpeed;
    motor.write(motorAngle);
    delay(30);
    SerialOutput(motorAngle, CalculateDistance());    
    motorAngle += motorRotateAmount;
    if(motorAngle <= minimumAngle || motorAngle >= maximumAngle) {
        motorRotateAmount = -motorRotateAmount;
    }}
int CalculateDistance(void)
{
    //digitalWrite(soundTriggerPin, LOW);
    //delayMicroseconds(2);
    digitalWrite(soundTriggerPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(soundTriggerPin, LOW);
      long duration = pulseIn(soundEchoPin, HIGH);
    float distance = duration * 0.017F;
    return int(distance);
}
void SerialOutput(const int angle, const int distance)
{
    String angleString = String(angle);
    String distanceString = String(distance);
    Serial.println(angleString + "," + distanceString);
}