Прямолинейное движение робота?

nitrior
nitrior аватар
Offline
Зарегистрирован: 02.02.2015

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

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

Подключил это дело, через модуль 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++;
}
//---------------------------------------

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

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

 

nitrior
nitrior аватар
Offline
Зарегистрирован: 02.02.2015

Забыл добавить:

За один оборот колеса происходит примерно 1000 срабатываний энкодера.

Один оборот колеса происходит примерно за 1 секунду.

В мониторе порта скорость выравнивается, а робота всеравно уводит в сторону.

lee
Offline
Зарегистрирован: 13.03.2014

Если количество оборотов в секунду равны, то возможно либо редукторы разные либо шасси кривое.

nitrior
nitrior аватар
Offline
Зарегистрирован: 02.02.2015

lee пишет:

Если количество оборотов в секунду равны, то возможно либо редукторы разные либо шасси кривое.

На одном моторе стоит пластиковая шестеренка, на другом латунная, но они одинаковые по размеру и количеству зубьев, шасси проверял стоят ровно. Может под нагрузкой что-то меняется.

Michal
Michal аватар
Offline
Зарегистрирован: 26.04.2013

импульсы от енкодеров не глотает программа?

У меня дачтик был на 96000 меток/оборот, железяка на малой скорости работала нормально, а на полном ходу ж*па. Не хватало быстродействия, пришлось аппаратный счетчик замутить

lee
Offline
Зарегистрирован: 13.03.2014

Признаюсь, не в курсе назначения штуковины, но может просто ввести поправочную константу (подобрать значение) как множитель значения ШИМ-а одного из двигателей (возможно коэффициент будет разный на разные скорости). Можно компас поставить, он будет подруливать )))

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

jeka_tm
jeka_tm аватар
Offline
Зарегистрирован: 19.05.2013

странный код честно сказать, точнее принцип. ну допустим все хорошо и после 100 миллисекунд скорость не меняется пока остальное делается, а если меняется? может отсюда вылезает проблема, импульсов одинаковое количество, а едут по разному

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

Gres
Gres аватар
Offline
Зарегистрирован: 26.03.2013

Тут Вы установили ШИМ на максимум, полные обороты:

nitrior пишет:

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

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

Шим 255 

И поехали вперед на полной скорости:

nitrior пишет:

// левый мотор вперед

digitalWrite(mot_in1, HIGH);

digitalWrite(mot_in2, LOW);

analogWrite(mot_ena, 255);

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

// правый мотор вперед

digitalWrite(mot_in3, HIGH);

digitalWrite(mot_in4, LOW);

analogWrite(mot_enb, 255);

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

 

А тут Вы пытаетесь добавить еще скорости(skor_lev++;), а больше некуда....:

nitrior пишет:

 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++; 

Вообще, Вы бы для начала убрали все лишнее из кода и отработали чисто движение. Убавьте ШИМ, скажем до 75% примерно для начала.

nitrior пишет:

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

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

Шим 190 

Gres
Gres аватар
Offline
Зарегистрирован: 26.03.2013

nitrior

Вот минимум кода, вывод ШИМ 75% на 9 пин и осцилограмма на этом пине, пляшите от этого:

#define mot_ena 9 //пин ШИМа левого мотора

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

void setup() {

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

  // чуток курим
  delay(100);

  // левый мотор вперед
  analogWrite(mot_ena, skor_lev);
}

void loop() {

  analogWrite(mot_ena, skor_lev); 
}

ШИМ 75% (десятичное значение 190)

nitrior
nitrior аватар
Offline
Зарегистрирован: 02.02.2015

jeka_tm пишет:

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

замерил время между импульсами функцией pulseIn(), оно постоянно пляшет, выдает то 182, то 190, а то и 60

jeka_tm
jeka_tm аватар
Offline
Зарегистрирован: 19.05.2013

зачем тебе pulsein если есть micros