Ultradzwiękowy czujnik odległości HC-SR04

W ręce trafił mi taki oto czujnik więc opiszę jak się do niego "dobrać".  Na pewno się komuś przyda na przykład do konstrukcji robotów.

 

HC-SR04 to ultradźwiękowy czujnik odległości. Ma jedno wejście (Trig) i jedno wyjście (Echo). Zasilany musi być napięciem +5V. Aby dokonać pomiaru należy na wejściu Trig ustawić +5V na czas co najmniej 10us. Na wyjściu Echo dostaniemy wówczas impuls o szerokości proporcjonalnej do odległości od przeszkody. Piny Trig i Echo możemy podłączyć do dowolnego, wolnego pinu na płytce STM32F4Discovery. Ja akurat Trig podłączyłem do PC2, a Echo do PC1:

No dobra. Czas napisać jakiegoś kota. Do obsługi Pina Trig będziemy potrzebowali OutputPort, a pomiar szerokości impulsu pinu Echo zrealizujemy na InterrputPort. Zapamiętamy czas wystąpienia narastającego i opadającego zbocza. Szerokość impulsu to różnica tych czasów. Klasa realizująca obsługę czujki wygląda następująco:

public class HCSR04 : IDisposable
{
    private readonly ManualResetEvent _resetEvent = new ManualResetEvent(false);
    private readonly object _syncRoot = new object();
    private bool _disposing;

    private static InterruptPort _echo;
    private static OutputPort _trigger;
    private long _startTime;
    private long _stopTime;

    public HCSR04(Cpu.Pin triggerPin, Cpu.Pin echoPin)
    {
        _echo = new InterruptPort(echoPin, false,
                                  Port.ResistorMode.PullDown,
                                  Port.InterruptMode.InterruptEdgeBoth);
        _trigger = new OutputPort(triggerPin, false);

        _echo.OnInterrupt += (port, state, time) =>
                                 {
                                     if (state == 0)
                                     {
                                         _stopTime = time.Ticks;
                                         _resetEvent.Set();
                                         return;
                                     }

                                     _startTime = time.Ticks;
                                 };
    }

    public TimeSpan Ping()
    {
        lock (_syncRoot)
        {
            if (!_disposing)
            {
                _trigger.Write(false);
                _startTime = _stopTime = 0;

                _resetEvent.Reset();
                _trigger.Write(true);
                Thread.Sleep(1);
                _trigger.Write(false);
                _resetEvent.WaitOne(60, false);

                if (_startTime > 0 && _stopTime > 0)
                    return TimeSpan.FromTicks(_stopTime - _startTime);
            }
        }

        return TimeSpan.MaxValue;
    }

    public void Dispose()
    {
        lock (_syncRoot)
        {
            _disposing = true;

            _trigger.Dispose();
            _echo.Dispose();
        }
    }
}

Procedura Ping zwraca długość trwania impulsu na wyjściu Echo lub TimeSpan.MaxValue w przypadku niepowodzenia pomiaru. Wyjaśnienia wymaga użycie ManualResetEvent i instrukcji lock.

Najpierw lock. Procedura Ping może być wywoływana z różnych wątków jednocześnie. Próba wykonania kolejnego pomiaru (np. przez inny wątek) przed zakończeniem poprzedniego pomiaru zakłóciłaby cały proces. Dlatego instrukcją lock objęto kot, do którego wątki muszą mieć dostęp pojedynczo. Kolejny wątek, który będzie chciał wykonać pomiar, będzie czekał dopóki nie zakończy się wykonywanie tego bloku. 

Natomiast użycie ManualResetEvent pozwala, w jak najkrótszym czasie, na zakończenie procedury. Przed wysłaniem impulsu Trig resetEvent jest kasowany, a na opadającym zboczu impulsu ustawiany. W linijce z WaitOne w najgorszym razie program będzie więc czekał 60 milisekund. Jeśli zbocze opadające pojawi się wcześniej, to ustawienie resetEvent spowoduje szybsze przejście programu do następnej instrukcji. Oczywiście zamiast resetEvent można by było użyć Thread.Sleep(60), ale wówczas program czekał by zawsze 60 milisekund.

Do pełni szczęścia brakuje jeszcze dwóch statycznych procedur do zamiany długości impulsu na odległość:

public static float ToCentimeters(TimeSpan pulse)
{
    if (pulse.Equals(TimeSpan.MaxValue))
        return Single.MaxValue;

    float result = pulse.TotalMicroseconds()/58f;
    return result;
}

public static float ToInches(TimeSpan pulse)
{
    if (pulse.Equals(TimeSpan.MaxValue))
        return Single.MaxValue;

    float result = pulse.TotalMicroseconds()/148f;
    return result;
}

Teraz można już robić pomiary. Najpierw kawałek programu do wyświetlania odległości:

var sensor = new HCSR04(Stm32F4Discovery.FreePins.PC2,
                        Stm32F4Discovery.FreePins.PC1);

for (;;)
{
    TimeSpan pulse = sensor.Ping();

    float cm = HCSR04.ToCentimeters(pulse);
    string cmStr = cm.Equals(Single.MaxValue) ? "?" : cm.ToString("F1");

    float inch = HCSR04.ToInches(pulse);
    string inStr = inch.Equals(Single.MaxValue) ? "?" : inch.ToString("F1");

    Debug.Print("Distance: " + cmStr + " cm = " + inStr + " in");

    Thread.Sleep(1000);
}

Prościzna. Można też zamieniać odległość na częstotliwość i sterować wyjściem PWM tak, aby dioda mrugała szybciej jeśli zbliżamy się do przeszkody, a wolniej jeśli się oddalamy. Trochę przypomina to czujnik parkowania. Procedura zamiany na częstotliwość wygląda tak:

public static float ToFrequency(TimeSpan pulse)
{
    //0-30 cm --> 12-1 Hz

    const int barier = 30; //cm

    float cm = HCSR04.ToCentimeters(pulse);
    if (cm > barier)
        return 1;

    const int maxFreq = 12; //Hz

    float result = (cm*(1 - maxFreq))/barier + maxFreq;
    return result;
}

A właściwe użycie tak:

var sensor = new HCSR04(Stm32F4Discovery.FreePins.PC2,
                        Stm32F4Discovery.FreePins.PC1);

var distancePwm = new PWM(Cpu.PWMChannel.PWM_0, 1, 0, false);
distancePwm.Start();

for (;;)
{
    TimeSpan pulse = sensor.Ping();

    distancePwm.Frequency = ToFrequency(pulse);
    distancePwm.DutyCycle = 0.5;

    Thread.Sleep(200);
}

Konstruktorom robotów na pewno się przyda detektor kolizji. Niezależna klasa, która poprzez zdarzenie poinformuje o zbliżeniu się do przeszkody. W detektorze użyto timera do cyklicznego odpytywania czujnika HC-SR04. Detektor generuje zdarzenie w przypadku przekroczenia (zbliżenie i oddalenie) zadanej odległości od przeszkody.

public class CollisionDetector
{
    public delegate void StateChangedEventHandler(bool crash);
    public event StateChangedEventHandler StateChanged;

    public float Barier { get; set; }

    private readonly HCSR04 _sensor;
    private readonly Timer _timer;
    private bool _collision;

    public CollisionDetector(HCSR04 sensor, TimeSpan scanPeriod)
    {
        _sensor = sensor;
        _timer = new Timer(TimerTick, null, TimeSpan.Zero, scanPeriod);
    }

    private void TimerTick(object state)
    {
        TimeSpan pulse = _sensor.Ping();
            
        float distance = HCSR04.ToCentimeters(pulse);
        if(distance.Equals(Single.MaxValue))
            return;

        bool newState = distance <= Barier;
        if (_collision ^ newState)
        {
            _collision = newState;
            StateChangedEventHandler handler = StateChanged;
            if (handler != null)
                StateChanged(_collision);
        }
    }
}

Teraz czerwoną diodę zapalamy jeśli nadmiernie zbliżymy się do przeszkody, a gasimy jeśli odległość będzie bezpieczna. Nic nie stoi na przeszkodzie, aby zamiast diody sterować np. silniczkiem.

var sensor = new HCSR04(Stm32F4Discovery.FreePins.PC2,
                        Stm32F4Discovery.FreePins.PC1);

var crashLed = new OutputPort(Stm32F4Discovery.LedPins.Red, false);
var scanPeriod = new TimeSpan(0, 0, 0, 0, 100); //100ms
var detector = new CollisionDetector(sensor, scanPeriod) {Barier = 10}; //10cm
detector.StateChanged += crashLed.Write;

Thread.Sleep(Timeout.Infinite);