Arduino.ru

Всем здравствуйте, возникла проблема с заменой delay на millis. Пока все безутешно. Код рабочий, но мне бы не хотелось, чтобы программа стопорилась на 4 секунды.
millis мне нужен для того, чтобы сервомотор выполнил оборот на 180 град, подождал 4 сек. и вернулся обратно в 0 град.

//шина I2C соответствует контактам A4, A5 в Ардуино UNO
//ПИНЫ A4, A5 / 20, 21 I2C ВЗАИМОСВЯЗАНЫ!! ДЛЯ СЧИТЫВАНИЯ ДАТЧИКА НАЖАТИЯ КТО-ТО ДОЛЖЕН БЫТЬ СВОБОДЕН
#include <Wire.h>
#include <Servo.h>
#include "Ultrasonic.h"
#define ttp229 (0xAF >> 1)
#define STEPPER_MICROSTEPS 8
#define STEPPER_PAUSE 120
#define STEPPER_DEGREES_PER_STEP 3.75

////////////////////////////////////////////////////////////////////////
//НАЗНАЧИМ ЦИФРОВЫЕ ПИНЫ 3 И 4 КАК DIR И STEP УПРАВЛЕНИЕ ДРАЙВЕРА МОТОРА #1
#define DIR_PIN A1
#define STEP_PIN A0
int m = 0;

////////////////////////////////////////////////////////////////////////
//ПЕРЕМЕННЫЕ КНОПКИ
uint16_t data_out = 0;
uint16_t data1, data2;

int count = 0;

boolean count2 = false;

int button[16];
int oldata[16];

///////////////////////////////////////////////////////////////////////
//ПЕРЕМЕННЫЕ РЕЛЕ
int RelayA = A3;
int RelayB = A2;
int n = 0;

///////////////////////////////////////////////////////////////////////
//ДАТЧИК РАССТОЯНИЯ
Ultrasonic ultrasonic(7, 6); // Trig - 6, Echo - 5
boolean flag = true;//ФЛАГ ПОМОЖЕТ ВЫРУБИТЬ МОТОР КОГДА СРАБОТАЕТ ДАТЧИК
uint32_t timer1;
#define PERIOD 350

///////////////////////////////////////////////////////////////////////
//СЕРВОМОТОР
Servo servo;  // create servo object to control a servo
int srv = 0;
uint32_t timer2;
#define PAUSE 3000

///////////////////////////////////////////////////////////////////////

void setup()
{
  pinMode(DIR_PIN, OUTPUT);
  pinMode(STEP_PIN, OUTPUT);

  pinMode(RelayA, OUTPUT);
  pinMode(RelayB, OUTPUT);

  servo.attach(9);   // пин контроллирующий сервомотор
  servo.write(180);
  
  Serial.begin(9600);
  Wire.begin(); 
}

void rotate(int steps, float speed)
{//НАЧАЛО
   int dir = (steps > 0)? HIGH:LOW;
   steps = abs(steps);
   digitalWrite(DIR_PIN,dir);
   float usDelay = (1/speed) * 70;
   for(int i=0; i < steps; i++ && flag == false)
   {
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(usDelay);

    digitalWrite(STEP_PIN, LOW);
    delayMicroseconds(usDelay); 

    /////////////////////////////////////////////////////////
    //Работа ULTRASONIC
    if(millis() - timer1 >= PERIOD)
    {
      float dist_cm = ultrasonic.Ranging(CM);                 //отображать расстояние в "см"
      Serial.println(dist_cm);                                //отображать расстояние
      if (dist_cm <= 10)
      {
        flag = true;
        break;
      }
      do
      {
        timer1 += PERIOD;
        if (timer1 < PERIOD) break;  // переполнение uint32_t
      } while (timer1 < millis() - PERIOD); // защита от пропуска шага
    }
     /////////////////////////////////////////////////////////
     //SERVO открывает лоток
     servo.write(1);                                                //ВКЛ. SERVO
     /////////////////////////////////////////////////////////
   }
}//КОНЕЦ

void loop()
{
  ////////////////////////////////////////////////////////////////////////
  //ВКЛЮЧИТЬ РЕЛЕ НА 1 РАЗ И ПОСЛЕ НАЖАТИЯ КНОПКИ ОБЕСТОЧИТЬ МОДУЛЬ КНОПОК
  while (n==0)
  {
    digitalWrite(RelayA, HIGH);
    digitalWrite(RelayB, HIGH);
    n++;
  }
  ////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////
  //Работа кнопок
  Wire.requestFrom(ttp229, 2, true);//функция запрашивает данные у ведомого устройства ttp229
                                    //7-битный адрес ведомого устройства - ttp229
                                    //количество запрашиваемых байт - 2
                                    //отправлен запрос со стоповым битом - true
  while(Wire.available())//возвращает количество байт
  {
    data1 = Wire.read();//считывает байт данных
    data2 = Wire.read();
    
    data_out = (data1 << 8) | data2;
    //////////////////////////////////////////////////////////////////
    for(int i = 1; i < 17; i ++)//Все кнопки
    {
      uint16_t contrast = 0x8000;
      if(data_out & contrast)
      {
        Serial.println(i);
        button[count] = i;
        count ++;
      }
      data_out <<= 1;    
    }
    ///////////////////////////////////////////////////////////////////
    if(count2)
    {
      switch(count)
      {     
        ////////////////////////////////////////////////////////////////
        //Если нажали одновременно 1-у кнопку
        case 1:
          if(oldata[0] == 1 && oldata[0] == button[0])//кнопка 1
          {
            digitalWrite(RelayA, LOW);
            digitalWrite(RelayB, LOW);
            Serial.println(button[0]);
            flag = false;
            /////////////////////////////////////////////////////////
            //Работа STEPPER
            if (flag == true)
              rotate(0, 0);                                             //ВЫКЛ STEPPER
  
            else 
              rotate(1600, 0.001);                                      //ВКЛ STEPPER
            /////////////////////////////////////////////////////////
            delay (4500);//////////////////////////////////////////// ЭТО НУЖНО ЗАМЕНИТЬ НА MILLIS()
            servo.write(180);
          }

          break;
        ////////////////////////////////////////////////////////////////
      }
      //////////////////////////////////////////////////////////////
    }
    ///////////////////////////////////////////////////////////////////////////////
    for(int i = 0; i < 16; i ++)
    {
      oldata[i] = button[i];
    }
    count = 0;
    count2 =! count2;
    ///////////////////////////////////////////////////////////////////////////////
  }
}