Arduino.ru

Доброго времени суток, господа. Хочу сразу предупредить что не силен в программирований. В общем столкнулся с такой задачей, что нужно установить обратную связь с частотником который управляет двигателем. Двигатель нагнетает воздух в трубу, а уже в трубе есть шарик. Который, естественно, поднимается вверх или опускается в зависимости от того как меняеться частота. 

Так же в трубе есть отверстия и что то вроде заслонки в ввиде диафрагмы фотоапарата. Таким образом при перекрываний одних из этих штук шарик поднимается. 

Задача в том чтобы заставить Ардуино снизить или повысить частоту чтобы ранне упомянутый шар стоял на месте.

Мое творение работает с ЦАПом так как аналоговые входы ПЧ реагирует только на 0-10В. И расчитывает положение шарика с помощью Ультрозвукового датчика дальности HС-SR501. Все устройства работают нормально, но возникла проблема в коде. 

После того как МК получает данные с датчика он решает повысить или понизить напряжение. Реализовал я это с помощью оператора for. Выглядит это так:

for (int i=level; dis<q; i++){  
        K = (byte(i));
        digitalWrite(2, bitRead(K, 7));
        digitalWrite(3, bitRead(K, 6));
        digitalWrite(4, bitRead(K, 5));
        digitalWrite(5, bitRead(K, 4));
        digitalWrite(6, bitRead(K, 3));
        digitalWrite(7, bitRead(K, 2));
        digitalWrite(8, bitRead(K, 1));
        digitalWrite(9, bitRead(K, 0));
        delay(500);
 }

 for (int i=level; dis>q; i--) {
        K = (byte(i));
        digitalWrite(2, bitRead(K, 7));
        digitalWrite(3, bitRead(K, 6));
        digitalWrite(4, bitRead(K, 5));
        digitalWrite(5, bitRead(K, 4));
        digitalWrite(6, bitRead(K, 3));
        digitalWrite(7, bitRead(K, 2));
        digitalWrite(8, bitRead(K, 1));
        digitalWrite(9, bitRead(K, 0));
        delay(500);
 }

После того как он проверяет условие и начинает повышать или понижать его уже не остановить. В идеале хотелось бы добиться чтобы МК после повышения значения i на 1 еденицу снова мерил растояние шарика и проверял условие. Этого я добиться не смог. Прошу помощи у адекватных професионалов, которые объяснят как же мне этого добиться. Заранее спасибо! 

Полный код (если кому интересно) приведен ниже.

#define trigPin 10
#define echoPin 11
#define keyPin 12
#define diodPin 13
unsigned int K=0;
unsigned int Q;
long dur;
int dis;
int q=0;
 
void setup() {
//Распиновка
 Serial.begin(9600);
 pinMode(2, OUTPUT);
 pinMode(3, OUTPUT);
 pinMode(4, OUTPUT);
 pinMode(5, OUTPUT);
 pinMode(6, OUTPUT);
 pinMode(7, OUTPUT);
 pinMode(8, OUTPUT);
 pinMode(9, OUTPUT);
 pinMode(trigPin, OUTPUT);
 pinMode(echoPin, INPUT);
 pinMode(keyPin, INPUT);


}

void loop() {

// Начало Работы Датчика
      digitalWrite(trigPin, LOW);
      delayMicroseconds(2);
      digitalWrite(trigPin, HIGH);
      delayMicroseconds(10);
      digitalWrite(trigPin, LOW);
 dur = pulseIn(echoPin, HIGH);
 dis = (dur/29.1)/2;
 dis = constrain(dis,0,50);
int level = map(dis,0,50,0,255);


 //Запоминание 

  if (keyPin, HIGH) {
  digitalWrite(diodPin, HIGH);
  delay(500);
  digitalWrite(diodPin, LOW);
  int q=level; 
 }
 
 //Условия
 
 for (int i=level; i<q; i++){  
        K = (byte(i));
        digitalWrite(2, bitRead(K, 7));
        digitalWrite(3, bitRead(K, 6));
        digitalWrite(4, bitRead(K, 5));
        digitalWrite(5, bitRead(K, 4));
        digitalWrite(6, bitRead(K, 3));
        digitalWrite(7, bitRead(K, 2));
        digitalWrite(8, bitRead(K, 1));
        digitalWrite(9, bitRead(K, 0));
        delay(500);
 }

 for (int i=level; i>q; i--) {
        K = (byte(i));
        digitalWrite(2, bitRead(K, 7));
        digitalWrite(3, bitRead(K, 6));
        digitalWrite(4, bitRead(K, 5));
        digitalWrite(5, bitRead(K, 4));
        digitalWrite(6, bitRead(K, 3));
        digitalWrite(7, bitRead(K, 2));
        digitalWrite(8, bitRead(K, 1));
        digitalWrite(9, bitRead(K, 0));
        delay(500);
 }
 

// Вывод данных на монитор
   Serial.print(dis);
   Serial.println("cm");
   

   delay(500);
 
 }