41 lines
1007 B
C++
41 lines
1007 B
C++
#include "ultrasound.h"
|
||
|
||
Servo Ultrasonic::servo;
|
||
int Ultrasonic::servoPin = 0;
|
||
|
||
UltrasonicPin Ultrasonic::pin = {0, 0};
|
||
float Ultrasonic::distance = 0;
|
||
int Ultrasonic::angle = 0;
|
||
|
||
void Ultrasonic::init(UltrasonicPin pin, int servoPin)
|
||
{
|
||
Ultrasonic::pin = pin;
|
||
Ultrasonic::servoPin = servoPin;
|
||
pinMode(pin.trig, OUTPUT);
|
||
pinMode(pin.echo, INPUT);
|
||
|
||
servo.attach(servoPin);
|
||
servo.write(90);
|
||
}
|
||
|
||
void Ultrasonic::update()
|
||
{
|
||
digitalWrite(pin.trig, HIGH);
|
||
delayMicroseconds(1);
|
||
digitalWrite(pin.trig, LOW);
|
||
float distance = pulseIn(pin.echo, HIGH); // 计数接收高电平时间
|
||
distance = distance * 340 / 2 / 10000; // 计算距离 1：声速：340M/S 2：实际距离为1/2声速距离 3：计数时钟为1US//温补公式：c=(331.45+0.61t/℃)m•s-1 (其中331.45是在0度）
|
||
Ultrasonic::distance = distance;
|
||
}
|
||
|
||
void Ultrasonic::servoControl(int angle)
|
||
{
|
||
if (angle < 0)
|
||
angle = 0;
|
||
if (angle > 180)
|
||
angle = 180;
|
||
|
||
Ultrasonic::angle = angle;
|
||
|
||
servo.write(angle);
|
||
} |