Arduino.ru

 Здравствуйте, помогиет пожалуйста разобраться. При добавлении в программу цикла for для вращения сервомашинки обновление подключенного датчика становится очень медленным, от этого робот уже давно врежится в стену. Не хватает оперативной памяти или частоты? Может код неверный?

#include <Servo.h>
const int Trig = 2; 
const int Echo = 4; 
Servo LexaServo;
int servoPin = 10;
struct MOTOR // структура для хранения номеров pin-ов, к которым подключены моторчики
{
int in1; // INPUT1
int in2; // INPUT2
int enable; // ENABLE1
};
// определяем порты, к которым подключены моторчики
MOTOR MOTOR1 = { 13, 12, 6 };
MOTOR MOTOR2 = { 7, 8, 5 };

void setup()
{
  pinMode(Trig, OUTPUT); 
  pinMode(Echo, INPUT); 
  pinMode(MOTOR1.in1, OUTPUT); // настраиваем выводы
  pinMode(MOTOR1.in2, OUTPUT); // на ВЫВОД
  pinMode(MOTOR2.in1, OUTPUT);
  pinMode(MOTOR2.in2, OUTPUT);
  LexaServo.attach(servoPin);
  Serial.begin(9600); 
}

unsigned int time_us=0;

unsigned int distance_sm=0;

void loop()
{
    digitalWrite(Trig, HIGH); // Подаем сигнал на выход микроконтроллера 
    delayMicroseconds(10); // Удерживаем 10 микросекунд 
    digitalWrite(Trig, LOW); // Затем убираем 
    time_us=pulseIn(Echo, HIGH); // Замеряем длину импульса 
    distance_sm=time_us/58; // Пересчитываем в сантиметры 
    Serial.println(distance_sm); // Выводим на порт
       if (distance_sm<15) // Если расстояние менее 15 сантиметром 
    { 
      back1(); //первый движок назад
      forward2(); //второй движок вперед
      delay(1000); //в течении 1 секунды
    } 
    else 
    { 
      forward1(); // вращаем оба моторчика вперёд
      forward2();
    }
    for (int i=0; i<=180; i=i+20)
  {
    LexaServo.write(i);
    delay(400);
  }
}

void forward1() // первый вперёд
{
  digitalWrite(MOTOR1.in1, HIGH);
  digitalWrite(MOTOR1.in2, LOW);
  analogWrite(MOTOR1.enable, 254);
}

void forward2() // второй вперёд
{
  digitalWrite(MOTOR2.in1, HIGH);
  digitalWrite(MOTOR2.in2, LOW);
  analogWrite(MOTOR2.enable, 254);
}

void back1() // первый назад
{
  digitalWrite(MOTOR1.in1, LOW);
  digitalWrite(MOTOR1.in2, HIGH);
  analogWrite(MOTOR1.enable, 254);
}

void back2() // второй назад
{
  digitalWrite(MOTOR2.in1, LOW);
  digitalWrite(MOTOR2.in2, HIGH);
  analogWrite(MOTOR2.enable, 254);
}