Arduino.ru

Добрый вечер!

Имеются два моторредуктора со встроенными оптическими энкодерами, моторы крутятся с разной скоростью, вследствии чего робота уводит в сторону, а надо чтобы он ехал прямо.

Подключил это дело, через модуль L298N к ардуино, набросал такой алгоритм выравнивания скоростей моторов:

Написал такой скетч:

#define mot_ena 9 //пин ШИМа левого мотора
#define mot_in1 8 //пин левого мотора
#define mot_in2 7 //пин левого мотора
#define mot_in3 5 //пин правого мотора
#define mot_in4 4 //пин правого мотора
#define mot_enb 6 //пин ШИМа правого мотора

volatile int lev_enc = 0; // переменная для левого энкодера
volatile int pra_enc = 0; // переменная для правого энкодера

byte skor_lev = 255; // ШИМ на левом моторе
byte skor_pra = 255; // ШИМ на правом моторе

byte max_skor = 254; // Не менять


void setup() {

  // пины энкодеров на вход
  pinMode(3, INPUT);
  pinMode(2, INPUT);
  //-------------------------

  //----------временно----------
  Serial.begin(9600);

  //-----------------------------

  // пины для левого и правого моторов на выход  
  pinMode(mot_ena, OUTPUT);
  pinMode(mot_in1, OUTPUT);
  pinMode(mot_in2, OUTPUT);
  pinMode(mot_in3, OUTPUT);
  pinMode(mot_in4, OUTPUT);
  pinMode(mot_enb, OUTPUT);  
  //-------------------------------------------

  delay(3000);

  // левый мотор вперед
  digitalWrite(mot_in1, HIGH);
  digitalWrite(mot_in2, LOW);
  analogWrite(mot_ena, 255);
  //---------------------------

  // правый мотор вперед
  digitalWrite(mot_in3, HIGH);
  digitalWrite(mot_in4, LOW);
  analogWrite(mot_enb, 255);
  //---------------------------

}


void loop() {

  // прямолинейное движение
  lev_enc = 0;
  pra_enc = 0;
  attachInterrupt(0, ENC_LEV, FALLING);
  attachInterrupt(1, ENC_PRA, FALLING);
  delay(100);
  detachInterrupt(0);
  detachInterrupt(1);  

  if (lev_enc != pra_enc)
  {
    if (pra_enc > lev_enc)
    {
      if (skor_pra > max_skor)
      {
        skor_pra--;
      }
      else
      {
        if (skor_lev > max_skor)
        {
          skor_pra--;
        }
        else
        {
          skor_lev++;
        }
      }   

    }
    else
    {
      // правая часть
      if (pra_enc < lev_enc)
      {
        if (skor_lev > max_skor)
        {
          skor_lev--;
        }
        else
        {
          if (skor_pra > max_skor)
          {
            skor_lev--;
          }
          else
          {
            skor_pra++;
          }
        }
      }

    }

  }
  else
  {
    // пусто
  }

  analogWrite(mot_ena, skor_lev); 
  analogWrite(mot_enb, skor_pra); 
  //-----------------------
//---------------------------
   Serial.print("Lev: ");
   Serial.print(lev_enc);
   Serial.print("   Pra: ");
   Serial.println(pra_enc);

   Serial.print("Lev sh: ");
   Serial.print(skor_lev);
   Serial.print("   Pra sh: ");
   Serial.println(skor_pra);

}

// функция считающая тики левого энкодера
void ENC_LEV () {
  lev_enc++;
}
//---------------------------------------

// функция считающая тики правого энкодера
void ENC_PRA () {
  pra_enc++;
}
//---------------------------------------

А робота всеравно уводит в сторону, меньше но уводит.

Что я делаю не так?