От Винта! Или РПО ВИШ на Arduino

#include <avr/wdt.h> // Подключаем библиотеку wdt

volatile unsigned long micros_prev = 0;  // Предидущее значение микросекунд
long rpm_sum = 0; // Сумма накопленых N значений оборотов
int rpm_val = 0; // Счетчик N
int rpm = 0; // Тахометр
int rpm_go = 0; // Заданные обороты
int rpm_select = 0; // Переключатель режимов
int rotor_left = 0; // Флаг вращения в лево
int rotor_right = 0; // Флаг вращения в право
int current = 0; // Датчик тока
int current_left = 0; // Флаг факта наличия вращения в лево
int current_right = 0; // Флаг факта наличия вращения в право

void setup() {
  wdt_disable(); // Отключаем wdt
  pinMode(2, INPUT); digitalWrite(2, HIGH); //  RPM Сигнал с тахометра и подтяжка к +
  pinMode(4, INPUT); // IS Ошибка драйвера (обработка сигнала не реализована)
  pinMode(7, INPUT); digitalWrite(7, HIGH); //  Step_down Малый шаг и подтяжка к +
  pinMode(8, INPUT); digitalWrite(8, HIGH); //  Step_up Большой шаги подтяжка к +
  pinMode(13, OUTPUT); // Светодиод - Ручной режим "красный"
  pinMode(3, OUTPUT); // INH Разрешить крутить эл.двигатель
  pinMode(5, OUTPUT); // IN 1-ый полумост
  pinMode(6, OUTPUT); // IN 2-ой полумост
  pinMode(12, OUTPUT); // Светодиод - Большой шаг "зеленый"
  pinMode(11, OUTPUT); // Светодиод - Малый шаг "зеленый"
  pinMode(10, OUTPUT); // Светодиод - Большой шаг отбойник "красный" упор
  pinMode(9, OUTPUT); // Светодиод - Малый шаг отбойник "красный" упор
  attachInterrupt(0, RPM_IN, FALLING); // Активируем внешнее прерывание
  wdt_enable (WDTO_2S); // Включаем wdt с интервалом 2 секунды
}

void loop() {
  wdt_reset(); // Сбрасывам счетчик wdt
  MODE();
  CURR();
  LED();
}

// Функция вращения в лево
void LEFT() {
  digitalWrite(5, HIGH);
  digitalWrite(6, LOW);
  digitalWrite(3, HIGH);
}

// Функция вращения в лево - скорость "MAX" (ШИМ)
void LEFT_MAX() {
  digitalWrite(5, HIGH);
  digitalWrite(6, LOW);
  analogWrite(3, 200);
}

// Функция вращения в лево - скорость "MIN" (ШИМ)
void LEFT_MIN() {
  digitalWrite(5, HIGH);
  digitalWrite(6, LOW);
  analogWrite(3, 120);
}

// Функция вращения в право
void RIGHT() {
  digitalWrite(5, LOW);
  digitalWrite(6, HIGH);
  digitalWrite(3, HIGH);
}

// Функция вращения в право - скорость "MAX" (ШИМ)
void RIGHT_MAX() {
  digitalWrite(5, LOW);
  digitalWrite(6, HIGH);
  analogWrite(3, 200);
}

// Функция вращения в право - скорость "MIN" (ШИМ)
void RIGHT_MIN() {
  digitalWrite(5, LOW);
  digitalWrite(6, HIGH);
  analogWrite(3, 120);
}

// Функция остановки эл.двигателя с торможением
void STOP() {
  digitalWrite(5, LOW);
  digitalWrite(6, LOW);
  digitalWrite(3, HIGH);
}

// Функция ручного режима
void HAND() {
  if (digitalRead(7) == LOW) {
    LEFT(); // Крутим эл.двигатель в лево если нажата кнопка в лево
    rotor_left = 1;
  } else {
    rotor_left = 0;
  }
  if (digitalRead(8) == LOW) {
    RIGHT(); // Крутим эл.двигатель в право если нажата кнопка в право
    rotor_right = 1;
  } else {
    rotor_right = 0;
  }
  if (rotor_left == 0 && rotor_right == 0) {
    STOP(); // Остановка эл.двигателя если не нажата ни одна кнопка
  }
}

// Функция автоматического режима
void AUTO() {
  if ((micros() - micros_prev) > 1000000) {
    rpm = 0;
  }
  // Отработка вращения в лево
  if (rpm < rpm_go - 60) {
    if (rpm < rpm_go - 150) {
      LEFT_MAX ();
    } else {
      LEFT_MIN ();
    };
    rotor_left = 1;
  } else {
    rotor_left = 0;
  }
  // Отработка вращения в право
  if (rpm > rpm_go + 60) {
    if (rpm > rpm_go + 150) {
      RIGHT_MAX ();
    } else {
      RIGHT_MIN ();
    };
    rotor_right = 1;
  } else {
    rotor_right = 0;
  }
  // Отработка диапазона "stop"
  if (rotor_left == 0 && rotor_right == 0) {
    STOP();
  }
}

// Функция обработки переключателя режимов работы
void MODE() {
  rpm_select = analogRead(0);
  if (rpm_select < 100) {
    rpm_go = 0;  // Ручной режим
  } else {
    if (rpm_select > 800) {
      rpm_go = 5700; // Автоматический режим "Взлет/Посадка"
    } else {
      if (rpm_select > 450) {
        rpm_go = 5300; // Автоматический режим "Набор высоты"
      } else {
        rpm_go = 4600; // Автоматический режим "Круиз"
      }
    }
  }
  if (rpm_go == 0) {
    HAND ();
  } else {
    AUTO ();
  }
}

// Функция распознавания направления вращения эл.двигателя
void CURR() {
  for (int i = 0; i < 4; i++) {
    current = current + analogRead(1);
  }
  current = current / 5;
  if (current < 496) {
    current_left = 1;
  } else {
    current_left = 0;
  }
  if (current > 516) {
    current_right = 1;
  } else {
    current_right = 0;
  }
}

// Функция отработки светодиодов
void LED() {
  if (rpm_go == 0) {
    digitalWrite(13, HIGH); // Ручной режим
  } else {
    digitalWrite(13, LOW);
  }
  // Если эл.двигатель крутим в лево а вращения нет, то включаем красный светодиод "Малый шаг" упор
  if (rotor_left == 1 && current_left == 0) {
    digitalWrite(11, LOW);
    digitalWrite(9, HIGH);
  }
  // Если эл.двигатель крутим в лево, то включаем зеленый светодиод "Малый шаг"
  else {
    if (rotor_left == 1) {
      digitalWrite(11, HIGH);
      digitalWrite(9, LOW);
    }
    else {
      digitalWrite(11, LOW);
      digitalWrite(9, LOW);
    }
  }
  // Если эл.двигатель крутим в право а вращения нет, то включаем красный светодиод "Большой шаг" упор
  if (rotor_right == 1 && current_right == 0) {
    digitalWrite(12, LOW);
    digitalWrite(10, HIGH);
  }
  // Если эл.двигатель крутим в право, то включаем зеленый светодиод "Большой шаг"
  else {
    if (rotor_right == 1) {
      digitalWrite(12, HIGH);
      digitalWrite(10, LOW);
    }
    else {
      digitalWrite(12, LOW);
      digitalWrite(10, LOW);
    }
  }
}

// Функция обработки сигнала с тахометра с усреднением данных
void RPM_IN() {
  rpm_sum = rpm_sum + (60000000.0 / (micros() - micros_prev));
  micros_prev = micros(); rpm_val++;
  if (rpm_val >= 10) {
    rpm = rpm_sum / 10;
    rpm_sum = 0;
    rpm_val = 0;
  }
}