Arduino и железнодорожный переезд из LEGO

// Светодиоды светофора:
int ledKrasnyj = 3; // Красный подключён через резистор 470 Ом к цифровому (digital)
                    // пину Ардуино с номером 5
int ledZheltyj = 13; // жёлтый - к тринадцатому ,
int ledZelenyj = 5; // зелёный - к третьему. Вторые ножки светодиодов подключаем к пину 5V.

// Будем использовать 2 сервомотора:
#include <Servo.h>
Servo ServoMotor_A; // Сервомотор A
Servo ServoMotor_B; // Сервомотор Б

int Ugol_A = 0;     /* Нам надо будет запоминать, на какой угол повёрнут сервомотор A,
 поэтому мы его будем записывать в переменную "Ugol_A" */
int Ugol_B = 0;     /* Нам надо будет запоминать, на какой угол повёрнут сервомотор Б,
 поэтому мы его будем записывать в переменную "Ugol_B" */

int UgolMAX = 65;   // Максимальный угол поворота шлагбаума

// DEBUG int Shlagbaum = 0;

/* Поскольку шлагбаумы к сервомоторам пререплены не очень ровно, приходится подобрать
 небольшую поправку к углам поворотов сервомоторов:
 */

int ServoMotorAPopravka = 0;
int ServoMotorBPopravka = 0;

/*
 Будем использовать фоторезистор.
 Контакт VCC подключаем к пину 5V, контакт GND - к пину GND.
 Последним подключаем контакт "AO" ("Analog output") к пину A0
 на Arduino:
 */

int FotoResistorPin = 0;

/*
 В зависимости от силы света на пине "A0" будут появляться значения от
 нуля до 1023. Ноль - это совсем светло (на датчик попадает яркий свет),
 1023 - очень темно (на датчик не попадает вообще никакого света).
 На старте считаем что датчик затемнён (будто поезд уже подъезжает к переезду):
 */

int FotoResistorSilaSveta = 1023;

/*
 На датчик обычно попадает чуточка света - так что ноль он обычно никогда не показывает.
 Надо договориться о каком-то числе - "отсечке": если датчик будет показывать больше,
 то будем считать, что сейчас темно, а если меньше - то светло.
 Это число надо подбирать опытным путём, потому что все датчики немножко отличаются.
 В нашем случае "отсечка" равна "650":
 */

int FotoResistorOtsechka = 650;

// Программа, которая гасит светофор:
void SvetoforVykl(){
  // Зажигаем один светодиод, другие - гасим
  digitalWrite(ledKrasnyj, LOW);
  digitalWrite(ledZheltyj, LOW);
  digitalWrite(ledZelenyj, LOW);
  Serial.println("[!] Svetofor: vikluchen");
}

// Программа, которая зажигает на светофоре красный:
void SvetoforKrasnyj(){
  digitalWrite(ledKrasnyj, HIGH);
  Serial.println("[!] Svetofor: KRASNYJ");
}

// Программа, которая зажигает на светофоре жёлтый:
void SvetoforZheltyj(){
  digitalWrite(ledZheltyj, HIGH);
  Serial.println("[!] Svetofor: ZHELTYJ");
}

// Программа, которая зажигает на светофоре зелёный:
void SvetoforZelenyj(){
  digitalWrite(ledZelenyj, HIGH);
  Serial.println("[!] Svetofor: ZELENYJ");
}

/* После того, как все настройки завершены, Arduino начнёт по кругу выполнять команды,
 которые хранятся в программе "loop" ("loop" с английского переводится как "круг"):
 */

void loop()
{
  // Сначала смотрим, что показывает датчик света.
  // Для этого считываем величину силы света из FotoResistorPin
  // в переменную "FotoResistorSilaSveta"

  FotoResistorSilaSveta = analogRead(FotoResistorPin);

  // Для удобства тестирования отправляем полученное значение в компьютер:
  Serial.print("[i] Datchik sveta pokazyvaet: ");
  Serial.println(FotoResistorSilaSveta);

  /* Теперь проверяем: полученное с датчика значение больше "отсечки" (то есть сейчас светло)
   или меньше отсечки (то есть сейчас темно)?
   */
  if(FotoResistorSilaSveta < FotoResistorOtsechka ){
    // Если светло, открываем шлагбаум и пишем в компьютер:
    Serial.println("[+] Svetlo");
    // И открываем шлагбаум:
    //if (Shlagbaum = 0){
      OtkrytSchlagbaum();
    //}
    // SvetoforZelenyj();
  }
  else{
    // Ну а если темно, закрываем и тоже пишем в компьютер:
    Serial.println("[-] Temno");
    // И закрываем шлагбаум:
    //if (Shlagbaum = 1){
      ZakrytSchlagbaum();
    //}
    // SvetoforKrasnyj();
  }

  // Чтобы не сильно мучить датчик, делаем короткую паузу перед тем, как всё начать снова:
  delay(1000);

}

// Программа, которая открывает шлагбаумы:
void OtkrytSchlagbaum(){
  //DEBUG if (Shlagbaum = 0){
    // Открываем шлагбаум:
    // SvetoforZheltyj();
    while ( (Ugol_A - ServoMotorAPopravka) < UgolMAX ){
      Ugol_A = Ugol_A + 1;
      Serial.print("Shlagbaum A: otkryvaem na ugol ");
      Serial.println(Ugol_A - ServoMotorAPopravka);
      ServoMotor_A.write(Ugol_A - ServoMotorAPopravka);
      delay(1);
    }

    while (  (Ugol_B - ServoMotorBPopravka) < UgolMAX    ){
      Ugol_B = Ugol_B + 1;
      Serial.print("Shlagbaum B: otkryvaem na ugol ");
      Serial.println(Ugol_B - ServoMotorBPopravka);
      ServoMotor_B.write(Ugol_B - ServoMotorBPopravka);
      delay(1);
    }
    delay(900);
    SvetoforVykl();
    SvetoforZelenyj();
  //DEBUG   Shlagbaum = 1;
    Serial.println("Shlagbaum OTKRYT");
  //DEBUG }
}

// Программа, которая закрывает шлагбаумы:
void ZakrytSchlagbaum(){
  // Закрываем шлагбаум:
  //DEBUG if (Shlagbaum = 1){
    // SvetoforZheltyj();
    while ( (Ugol_A - ServoMotorAPopravka) > 0 ){
      Ugol_A = Ugol_A - 1;
      if ((Ugol_A - ServoMotorAPopravka) >= 0){
        Serial.print("Shlagbaum A: zakryvaem na ugol ");
        Serial.println(Ugol_A  - ServoMotorAPopravka);
        ServoMotor_A.write(Ugol_A - ServoMotorAPopravka);
        delay(1);
      }
    }

    while ( (Ugol_B - ServoMotorBPopravka) > 0 ){
      Ugol_B = Ugol_B - 1;
      if ((Ugol_B - ServoMotorBPopravka) >= 0){
        Serial.print("Shlagbaum B: zakryvaem na ugol ");
        Serial.println(Ugol_B  - ServoMotorBPopravka);
        ServoMotor_B.write(Ugol_B - ServoMotorBPopravka);
        delay(1);
      }
    }
    delay(900);
    SvetoforVykl();
    SvetoforKrasnyj();
  //DEBUG   Shlagbaum = 0;
    Serial.println("Shlagbaum ZAKRYT");
  //DEBUG }
}

/*
 До этого момента мы только объявляли переменные.
 Теперь в служебной программе "setup" мы в них запишем
 стартовые значения. Программа "setup" выполняется Arduino один раз,
 когда устройство включается:
 */

void setup()
{

  Serial.begin(9600);      /* Говорим Arduino, что мы хотим отправлять сигналы в компьютер
   и читать их в PUTTY */

  // Светодиоды светофора:
  pinMode(ledKrasnyj, OUTPUT);
  pinMode(ledZheltyj, OUTPUT);
  pinMode(ledZelenyj, OUTPUT);


  /* Заканчиваем настройку сервомоторов - говорим Arduino,
   к каким пинам какой сервомотор подключён */
  ServoMotor_A.attach(9,  500, 2200);  // Контакт сервомотора А помечен как "Servo_2" на Motor shield
  ServoMotor_B.attach(10, 500, 2200);  // Контакт сервомотора Б помечен как "Ser1" на Motor shield

  // Приводим сервомоторы в стартовое положение:
  Serial.print("Ugol servomotora A: ");
  Serial.println(Ugol_A + ServoMotorAPopravka);
  ServoMotor_A.write(Ugol_A + ServoMotorAPopravka);

  Serial.print("Ugol servomotora B: ");
  Serial.println(Ugol_B + ServoMotorBPopravka);
  ServoMotor_B.write(Ugol_B + ServoMotorBPopravka);

  delay(3500);

  SvetoforKrasnyj();
  // ZakrytSchlagbaum();

}