#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);
}