Помогите доделать блок на СИ

[Balzamalex]

Здравствуйте. Сам не силен в СИ, но пытаюсь разобраться. Есть библиотека для дальномеров SR04 и SRF05. Знаю, что есть готовый блок, но мне интереснее самому сделать.
В принципе, я уже сделал, но подозреваю, что таааааакого там навертел.
Вот так выглядит пример:

СпойлерПоказать

// SENSOR DEMO
#include "SRF05.h"

// trigPin, echoPin, MaxDist, readInterval
SRF05 Sensor(6, 7, 200, 500);

void setup() {
Serial.begin(9600);
// If using SR04 enable the following line:
//Sensor.Unlock = true;
}

void loop() {
// Distance read
if ( Sensor.Read() > -1 ) {
// New distance reading!
if ( Sensor.Distance == 0 ) {
Serial.println("Out of range");
} else {
// -------------
Serial.print("Dist: ");
Serial.print(Sensor.Distance);
Serial.println(" cm");
}
}
}


Вот что получилось у меня:

Setup

СпойлерПоказать

// If using SR04 enable the following line:
Sensor.Unlock = true;

Loop


СпойлерПоказать

if  ( EN_SRF05  == HIGH)  // если разрешено на входе
{
// Distance read
if ( Sensor.Read() > -1 )
{
// New distance reading!
if ( Sensor.Distance == 0 ) {
distanceSRF05 = (  MaxDist  );
Out_of_range_SRF05   =  true;
}
else {
// -------------

distanceSRF05 = ( Sensor.Distance );
Out_of_range_SRF05   =   false;
}
}

}
else {
distanceSRF05 = (  MaxDist  );
Out_of_range_SRF05   =  false;
}

Как ни странно, ЭТО работает :-) , но прошу знатоков поправить и сделать как положено.
Меня очень смущает в Setup строка Sensor.Unlock = true; Подозреваю, что ее надо куда то в функции запихнуть, но вот как?

Добавлено (13.08.2016, 10:10)
---------------------------------------------
Целиком не влезло
Declare


СпойлерПоказать

#include "SRF05.h"
SRF05 Sensor( trigPin ,  echoPin ,  MaxDist ,  readInterval );