Синхронизация вращения колес у робота

[Юртрит]

И звените все я разобрался) не с этого я начал теперь едет ровно) спасибо

[Streg]

Admin, прошу помочь разобраться. Не могу добиться прямолинейности хода. Робота собрал в соответствии с вашей схемой. Использую arduino nano v.7 ,плату arduino, оптопара аналог Вашей TCST1103. С выхода оптопары имею сигнал мин 60мВ и максимум 140мВ, что как мне кажется не достаточно для ардуино. К тому-же хотел бы понять почему Вами была принята данная схема, а не рекомендуемое подключение оптопары к ардуино:

[Admin]

У меня практически такая же схема, я только для экономии одного резистора включил светодиоды последовательно

[Streg]

Что светодиоды последовательно, это я понял. Главный вопрос почему нет резистора и самой цепи питания в коллекторе фототранзистора? При этом бы и обеспечивалась напряжение на выходе оптопары макс 5В и мин 0В.

[Admin]

Он подключается программно, тот который внутри контроллера

[Streg]

Получается, что у меня не подключается? А Вы можете померить на своем роботе напряжение на D2 и D3 максимальное и минимальное при вращении энкодера? И может быть подскажите еще как можно проверить его работу.

[Paul_B]

Я думаю, у вас в программе надо исправить - прерывания должны сниматься с экнодеров и в поворотах, только в поворотах не надо выравнивать движение. Зато по разности счетчиков и разности управляющей скорости на внешнем и внутреннем колеах можно четко вычислять угол поворота.

[Admin]

В данной теме приведен пример, само собой углы поворотов можно задавать разностью импульсов в переменной course, желательно иметь две переменные по одной на каждое колесо

[Paul_B]

Сообщение от Admin

В данной теме приведен пример, само собой углы поворотов можно задавать разностью импульсов в переменной course, желательно иметь две переменные по одной на каждое колесо

Если робот едет прямолинейно, то можно каждые 100 единиц (5 оборотов колеса) обнулять два счетчика (левого колеса и правого) при длительном прямолинейном движении, и при начале прямолинейного движения, т.е. при завершении поворота. Далее, если опять же едет прямо, то при разнице в счетчиках на 2 единицы включать корректировку. Тут возникает вопрос - на сколько делать разность скоростей правых и левых колес для выравнивания разности (10% текущей скорости?).
А в поворотах уже считать реальные показания счетчиков и вычислять угол поворота, исходя из ширины базы робота, диаметра колеса и скорости внутреннего колеса.

Т.е. получается, что IntOn() должна включаться всегда, как какой-то двигатель начинает движение (чтобы не дублировать надо ввести булевую переменную, отражающую включена функция или нет), а IntOff() - когда оба двигателя останавливаются и при этом обнулять оба счетчика (левый и правый) и присваивать false той самой введенной переменной.

[Paul_B]

Вот какие есть задумки по движению с энкодерами... Можно задавать повороты в градусах, а движения в сантиметрах.
Робота пока жду, поэтому только в теории.

#include <MsTimer2.h>   //библиотека для работы с таймером-2 (для опроса ИК-датчиков и отсчета времени
#include <Servo.h>   //используем библиотеку для работы с сервоприводом
#include <avr/wdt.h>    //подключение библиотеки для работы со сторожевым таймером
#include <IRremote.h>   //библиотека для ИК приемника

#define size_buff 5
#define trigPin 12
#define echoPin 11

#define ServoF 10
#define ML_E 2  // энкодер левого двигателя
#define MR_E 3  // энкодер правого двигателя

#define ML_1 4  // левый двигатель 
#define ML_2 5
#define ML_S 6
#define MR_1 7  // правый двигатель 
#define MR_2 8
#define MR_S 9
#define Sound 13

Servo Servo_F;  //объявляем переменную servo типа Servo
uint8_t U_Servo; // активный угол серво


char Napr[2] = {' ', ' '}; //выполняемое движение ячейка [0] - задает направление F/B, ячейка [1] - поворот R/L
uint8_t SPEED, OLD_SPEED, //переменные для текущей скорости и прошлой скорости
        L_SPD = 50, // медленная скорость
        M_SPD = 150,      // средняя скорость
        F_SPD = 255;      // максимальная скорость
        
bool LR_count = false;  // флаг работы счетчиков энкодеров
bool STOP_L=false, STOP_R=false; // признак притормаживания соотв. колеса (для энкодеров)
uint16_t ML_count, MR_count; // счетчики правого и левого энкодеров
uint16_t LimD_count,    // предельное значение счетчика, на котором при движении вперед нет препятствий
         LimF_count,    // предельное значение счетчика, если задан отрезок прямолинейного движения
         LimL_count,    // значение счетчика левого колеса при правом повороте на заданный угол 
         LimR_count;    // значение счетчика правого колеса при левом повороте на заданный угол
uint16_t TurnL_count=0, TurnR_count=0; // счетчик завершенного поворота

uint16_t sensor[size_buff], R_time;
uint16_t IR[4] = {A2, A3, A4, A5};
uint16_t IR_Dist[4];

// для расчета расстояний из счетчика энкодера
uint16_t BAZA = 120; //ширина между колесами в mm (середин колес)
uint16_t KOLESO = 65; //диаметр колеса в mm
uint16_t N_Encoder = 20; //количество отверстий в энкодере
float Pi = 3.1415926;

void setup() {
  uint8_t i;
  Serial.begin(9600); // инициализация послед. порта

  Servo_F.attach(ServoF);
  Servo_Ugol('F', 90, 1);

  pinMode(trigPin, OUTPUT); // триггер - выходной пин
  pinMode(echoPin, INPUT); // эхо - входной

  pinMode (ML_1, OUTPUT);
  pinMode (ML_2, OUTPUT);
  pinMode (ML_S, OUTPUT);
  pinMode (ML_E, INPUT);

  pinMode (MR_1, OUTPUT);
  pinMode (MR_2, OUTPUT);
  pinMode (MR_S, OUTPUT);
  pinMode (MR_E, INPUT);

  pinMode (Sound, OUTPUT);

  for (i = 0; i < 4; i++)
    pinMode(IR[i], INPUT);
  
  R_time = 0;
  SPEED = 0;
  MsTimer2::set(500, IR_Move());  // прерывания по таймеру, период 500 мс
  MsTimer2::start();              // разрешение прерывания

  IntOff();
  R_Stop();

  for (i = 0; i < 12; i++) beep(70, random(100, 2000)); //звуковое оповещение готовности робота

  wdt_enable(WDTO_1S);
  Serial.println("Baster ready!");
}

void loop() {
  // put your main code here, to run repeatedly:
  if (Serial.available())
  {
    Serial.read();
  }
  
  if (Napr[1] == 'E' || ML_count >= LimD_count) // закончился поворот или уже долго едем без проверки УЗД направления
    {
     Napr[1] = ' ';
     if (Napr[0] == 'F') SPEED=Forward_Distance(SPEED);
    }

  //uint16_t distance = GetDistance(); // получаем дистанцию с датчика
  //Serial.println(distance); // выводим в последовательный порт
  // delay(100);
  wdt_reset();

}

bool Servo_Ugol(char Serv, uint16_t ugol, bool Wt)
{
  if (Serv == 'F')
  {
    if (U_Servo == ugol) exit(0);
    Servo_F.write(ugol); //ставим вал под ugol
    if (Wt) delay(20+int(120 * abs(U_Servo - ugol) / 60)); // ждем пока вал повернется
    U_Servo = ugol;

  }
}

//************* определение минимального расстояния в секторе поворота / разворота *************
uint16_t GetSector(char Serv, uint16_t ugol)
{
  uint16_t i, dist, min_dist = 300;
  unsigned long t0,t1;
  
  t0=millis()+200;
  Servo_Ugol(Serv, ugol, false);  // начинаем поворачивать серву
  do {                            // и во время поворота снимаем данные с УЗД
      dist = GetDistance();     
      if(dist<min_dist) min_dist=dist; // самое минимальное расстояние отсылаем
      t1=millis();
  } while (t1<t0);
  return(min_dist);
}    

//************ Определение дистанции до объекта в см   ***************
uint16_t GetDistance()
{
  uint16_t dist;
  uint8_t i;

  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  for (i = 0; i < size_buff; i++) //производим несколько замеров
  {
    digitalWrite(trigPin, HIGH); // генерируем импульс запуска
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    dist = pulseIn(echoPin, HIGH, 12000); //считываем длительность времени прохождения эха, ограничить время ожидания
    if (dist == 0) dist = 12000;
    else delayMicroseconds(20);
    sensor[i] = dist; //сохранить в массиве
  }
  dist = (find_similar(sensor, size_buff, 58)) / 58; // //фильтруем показания датчика и переводим в см
  return dist;
}


//**************Поиск макс повторяющегося элемента в массиве ****************************
uint16_t find_similar(uint16_t *buf, uint8_t s_buff, uint8_t range)
{
  uint8_t maxcomp = 0; //счётчик максимального количества совпадений
  uint16_t mcn = 0; //максимально часто встречающийся элемент массива
  uint16_t comp;   //временная переменная
  uint8_t i, j, n; //счётчик совпадении
  uint8_t i_numb, i_mas[size_buff]; // индекс интервала и показатель принадлежности к заданному интервалу отклонений

  range++;   //допустимое отклонение
  for (i = 0; i < s_buff; i++) i_mas[i] = 0; // задаем начальные нули. чтобы сэкономить можно задать static uint8_t i_mas[256];
  i_numb = 1;  // задаем номер интервала. если измерение будет лежать в этом интервале, то в индексном массиве присваиваем этот номер

  for (i = 0; i < s_buff; i++)
    if (i_mas[i] == 0) // если элемент не попал ни в чей интервал
    {
      comp = buf[i]; //кладем элемент массива в comp
      n = 0;  //счётчик совпадении
      for (j = i; j < s_buff; j++)
        if (i_mas[j] == 0) // если раньше этот элемент не попал в какой-то интервал, то его рассматриваем
        {
          if (buf[j] > comp - range)
            if (buf[j] < comp + range) {
              i_mas[j] = i_numb;
              n++;
            }
        }

      if (n > maxcomp) //если число повторов больше чем было найдено ранее
      {
        maxcomp = n; //сохраняем счетяик повторов
        mcn = comp; //сохраняем повторяемый элемент
      }
      i_numb++;   // тут в итоге будет количество непересекающихся интервалов измерений
    }
  return mcn;
}

//************* прозвон через УЗД беспрепятственной дистанции вперед ***********
int Forward_Distance(uint8_t spd)
{
  int s;
  Servo_Ugol('F', 90, true);
  uint16_t dist = GetDistance(); // замеряем расстояние до препятствия по прямой
  LimD_count = max(0,ML_count + N_Encoder * 10 * (dist - 20) / (Pi * KOLESO)); //сколько можно ехать по прямой без УЗД

  if (dist < 100) s = min(M_SPD, spd);
  if (dist < 50) s = min(L_SPD, spd);
  if (dist < 20) s=-1;
  return (s);
}

//********* реакция на сработавшие ИК-датчики
int IR_Move()
{
  IR_Distance();
  // ....
  return (0);
}
//********* опрос ИК-датчиков (в т.ч. и по таймеру каждые 500мс) ************
void IR_Distance()
{
  uint16_t dist;
  uint8_t i, j;

  R_time++;  // счетчик времени с начала включения
  for (j = 0; j < 4; j++)
  {
    for (i = 0; i < size_buff; i++) //производим несколько замеров
    {
      dist = analogRead(IR[j]); // чтение напряжения на входе A3

      //обработка показанийб чтобы привести к сантиметрам

      sensor[i] = dist; //сохранить в массиве
    }
    //IR_Dist[j]=find_similar(sensor, size_buff, 1); // //фильтруем показания датчика и переводим в см
  }
}



void MotorL()  // обработка внешнего прерывания левое колесо
{
  ML_count++;
  if (SPEED == 0) exit(0);
  
  if(LimF_count>0) // есть ограничения на езду прямо
    if(ML_count>=LimF_count)
      {
       LimF_count=0;
       R_Stop();
      }

  if (Napr[1] == ' ') //когда не поворачиваем притормаживаем правый двигатель
    {
     if (MR_count - ML_count > 1) // т.е. правый двигатель крутится быстрее и 
       {
        if(!STOP_L) 
          {// подтормаживаем правый двигатель, если не подтормжен левый
           analogWrite (MR_S, int(SPEED / 2));// притормаживаем правый двигатель
           STOP_R=true;
          }
       }
     else {// курс ровный
           if(STOP_R) // если правый двигатель тормозился, возвращаем прежнюю сорость
             {
              analogWrite (MR_S, SPEED);  
              STOP_R=false;
             }
          }
     }  
  else {
        if(Napr[1]=='R') // задан поворот на фикированный угол 
           {
            if(ML_count-MR_count>=LimL_count)  // поворот на заданный градус завершен
              {
               TurnL_count=0; 
               LimL_count=0;
               analogWrite (MR_S, SPEED);  // восстанавливаем скорость на правом двигателе
               Napr[1]=='E'; // признак окончания поворота
               ML_count = 0; // начало прямолинейного движения, скидываем счетчики
               MR_count = 0;
              }
            else TurnL_count=MR_count-ML_count; // если поворачиваем, считаем на столько осуществлен поворот     
           }
         if(Napr[1]=='Z') // задан разворот   
            {
             if(ML_count>=LimL_count)  // поворот на заданный градус завершен
              {
               LimL_count=0;
               R_Stop();
              }
            }
        }

}

void  MotorR()  // обработка внешнего прерывания правое колесо
{
  MR_count++;
  if (SPEED == 0) exit(0);
  
  if(LimF_count>0) // есть ограничения на езду прямо
    if(MR_count>=LimF_count)
      {
       LimF_count=0;
       R_Stop();
      }

  if (Napr[1] == ' ') //когда не поворачиваем притормаживаем левый двигатель
    {
     if (ML_count - MR_count > 1) // т.е. левый двигатель крутится быстрее и 
       {
        if(!STOP_R) 
          {  // подтормаживаем левый двигатель
           analogWrite (ML_S, int(SPEED / 2));
           STOP_L=true;
          }
       }
     else {// курс ровный
           if(STOP_L) // если левый двигатель тормозился, возвращаем прежнюю сорость
             {
              analogWrite (ML_S, SPEED);  
              STOP_L=false;
             }
          }
    }  
  else {
        if(Napr[1]=='L') // задан поворот на фикированный угол 
           {
            if(MR_count-ML_count>=LimR_count)  // поворот на заданный градус завершен
              {
               TurnR_count=0; 
               LimR_count=0;
               analogWrite (ML_S, SPEED);  // восстанавливаем скорость на левом двигателе
               Napr[1]=='E'; // признак окончания поворота
               ML_count = 0; // начало прямолинейного движения, скидываем счетчики
               MR_count = 0;
              }
            else TurnR_count=MR_count-ML_count; // если поворачиваем, считаем на столько осуществлен поворот     
           }

        }
}



//********* остановка робота ************************
void R_Stop()
{
  if (LR_count) IntOff(); //выключаем счетки энкодеров
  analogWrite(ML_S, 0);       // останавливаем двигатели
  analogWrite(MR_S, 0);
  Napr[0]==' ';
  Napr[1]==' ';
  OLD_SPEED = SPEED;
  SPEED = 0;
}


//*********** установка двигателей на движение вперед ***
void Set_Forward()
{
  if (Napr[0] != 'F')
  {
    digitalWrite (ML_1, LOW);    // устанавливаем направление левого и правого двигателя
    digitalWrite (ML_2, HIGH);
    digitalWrite (MR_1, LOW);
    digitalWrite (MR_2, HIGH);
    Napr[0] = 'F';                //задаем символьное направление движения
  }
}


//*********** установка двигателей на движение назад ***
void Set_Back()
{
  if (Napr[0] != 'B')
  {
    digitalWrite (ML_1, HIGH);    // устанавливаем направление левого и правого двигателя
    digitalWrite (ML_2, LOW);
    digitalWrite (MR_1, HIGH);
    digitalWrite (MR_2, LOW);
    Napr[0] = 'B';                //задаем символьное направление движения
  }
}


//*********** движение робота назад либо на заданное число см, либо до упора *********
int R_Back(uint8_t spd, uint16_t len)
{
  if(Napr[0]!='B')
    {
     ML_count = 0; // начало прямолинейного движения, скидываем счетчики
     MR_count = 0;
     Set_Back(); //устанавливаем двигатели для движения назад
    }
      
  LimF_count = 0;
  if (len > 0) // если задано расстояние для движения назад
     LimF_count = int(N_Encoder * 10.0 * len / (Pi * KOLESO)); // значение счетчика для заданного расстояния

  Fast_slow(spd);   //плавно разгоняемся
  //  .....
  return (0);
}

//*********** движение робота вперед либо на заданное число см, либо до упора ********
int R_Forward(uint8_t spd, uint16_t len)
{
  int s;
  
  if(Napr[0]!='F')
    {
     ML_count = 0; // начало прямолинейного движения, скидываем счетчики
     MR_count = 0;
     Set_Forward(); //устанавливаем двигатели для движения вперед
    }
 
  if (Napr[0] != 'F' || Napr[1] != ' ' || ML_count >= LimD_count)
  {
    s = Forward_Distance(spd);
    if (s == -1) return (-1);
    spd = s;
  }
  
  LimF_count=0;
  if (len > 0) // если задано расстояние для движения вперед
    LimF_count = int(N_Encoder * 10.0 * len / (Pi * KOLESO)); // значение счетчика для движения а заданное расстояние

  Fast_slow(spd);   //плавно разгоняемся
  // ....
  return (0);
}

//*********** замедление или разгон с одной скорости на другую *********
uint8_t Fast_slow(uint8_t spd)
{
  uint8_t i, max_spd, min_spd, sh;
  unsigned long t0,t1;
  
  
  if (!LR_count) IntOn(); //включаем енкодеры, если они отключены
  if (spd == SPEED) return (spd); //если скорость не меняется, выходим из процедуры
  OLD_SPEED = SPEED;  //сохраняем значение старой скорости
  if(Napr[1]==' ')    // ксли не задан поворот, то плавно меняем изменение скорости
    {
      if (spd > SPEED)
      { max_spd = spd;
        min_spd = SPEED;
      }
      else {
        max_spd = SPEED;
        min_spd = spd;
      }
  

      if (SPEED >= M_SPD || spd >= M_SPD) // и скорость задана от средней, плавно увеличиваем или уменьшаем скорость
        {
          sh = (max_spd - min_spd) / 10;
    
          for (i = 1; i < 10; i++)
          {
            analogWrite (ML_S, min_spd + i * sh);
            analogWrite (MR_S, min_spd + i * sh);
            t0=millis()+100;
            do {                           
              // нужно вставить опрос ИК-датчиков при разгоне, чтобы не врезаться
                t1=millis();
            } while (t1<t0);
    
          }
        }
    }  
  analogWrite (ML_S, spd);
  analogWrite (MR_S, spd);
  SPEED = spd;
  return (spd);

}


//*************** поворот налево  *******************
int R_Left(uint16_t ugol, float Radius)
// Radius - задает радиус движения по окружности при повороте
// ugol - задает дугу окружности.
{
  
  if (SPEED == 0) return(-2);     //если робот не движется, поворота не будет

  // задается угол поворота. если угол зада 0, то включаетсяя поворот до тех пор, пка не будет принудительный выход из поворота
  if (!LR_count) IntOn();

  Napr[1] = 'L';
  if(ugol==0) ugol=360;

   if (Napr[0] == 'F')
     Servo_Ugol('F', max(0, 90 - int(ugol / 2)), false);

   analogWrite (ML_S, int(Radius*SPEED));    // тормозим левый двигатель
   LimR_count = int((ugol / 360.0) * N_Encoder * 2 * BAZA / KOLESO); // значение счетчика для поворота на заданный угол


}

int R_Right(uint16_t ugol, float Radius)
// Radius - задает радиус движения по окружности при повороте
// ugol - задает дугу окружности.
{
  if (SPEED == 0) return(-2);     //если робот не движется, поворота не будет

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

  if (!LR_count) IntOn();

  Napr[1] = 'R';
  if(ugol==0) ugol=360;
  
  if (Napr[0] == 'F')
    Servo_Ugol('F', min(180, 90 + int(ugol / 2)), false);

  analogWrite (MR_S, int(Radius*SPEED));    // тормозим правый двигатель
  LimL_count = int((ugol / 360.0) * N_Encoder * 2 * BAZA / KOLESO);
}


//******** Разворот на месте на угол (0-360) в заданную сторону ('L' 'R') **********
int Rotation(uint16_t ugol, char Side)
{
  uint16_t dist, i = 0;
  float x;
  R_Stop();
  if (!LR_count) IntOn();
  i = 0;
  while (i < 2)
  {
    if (Side == 'L')
    {
      dist = GetSector('F',0); // замеряем расстояние до препятствия в левом секторе
      if (dist < 15) {
        Servo_Ugol('F', 90, true); // нашли препятствие, возвращаем серву в центр и пробуем развернуться через право
        Side = 'R';
        ugol = 360 - ugol;
        i++;
      }
      else break;
    }
    if (Side == 'R')
    {
      dist = GetSector('F',180); // замеряем расстояние до препятствия в правом секторе 
      if (dist < 15) 
        {  // нашли препятствие, возвращаем серву в центр и пробуем развернуться через право
         if(i==0)   
           {  // если еще не пробовали разворот через лево, возвращаем серву в центр и пробуем развернуться через лево
            Side = 'L';
            ugol = 360 - ugol;
            Servo_Ugol('F', 90, true); // нашли препятствие, возвращаем серву в центр и пробуем развернуться через право
           }
        i++;
       }
      else break;
    }
  }
  Servo_Ugol('F', 90, false);
  if (i == 2) return (-1);    // через обе стороны нельзя развернуться

  if (Side == 'L')  //разворачиваемся через лево
  {
    digitalWrite (ML_1, HIGH);
    digitalWrite (ML_2, LOW);
    digitalWrite (MR_1, LOW);
    digitalWrite (MR_2, HIGH);
   }
  if (Side == 'R')
  { //разворачиваемся через право
    digitalWrite (ML_1, LOW);
    digitalWrite (ML_2, HIGH);
    digitalWrite (MR_1, HIGH);
    digitalWrite (MR_2, LOW);
  }
  Napr[1] = 'Z';
  LimL_count = (ugol / 360.0) * N_Encoder * BAZA / KOLESO;
  analogWrite (ML_S, int((L_SPD + M_SPD) / 2)); // разворачиваемся на средней скорости
  analogWrite (MR_S, int((L_SPD + M_SPD) / 2));

  R_Stop();
  return (0);
}


//***************************************************
void IntOn()
{
  LR_count = true;
  attachInterrupt(0, MotorL, RISING);  // настроить срабатывание прерывания interrupt0 pin 2 по перерпапду с низкого (Low) на высокий(HIGH)
  attachInterrupt(1, MotorR, RISING);  // настроить срабатывание прерывания interrupt1 pin 3 по перерпапду с низкого (Low) на высокий(HIGH)
}

void IntOff()
{
  LR_count = false;
  detachInterrupt(0);  // отключить срабатывание прерывания interrupt0
  detachInterrupt(1);  // отключить срабатывание прерывания interrupt1
}


//***************Beep от 50 до 2000Гц****************
void beep(uint16_t dur, uint16_t frq)
{
  uint16_t i;
  //dur=(1000/frq)*dur;  //расчет длительности бипа
  uint16_t per = 500000 / frq; //длит. полупер в мкс
  dur = dur / (per / 250) * 2;
  for (i = 0; i < dur; i++)
  {
    digitalWrite(Sound, HIGH);
    delay(per);
    digitalWrite(Sound, LOW);
    delay(per);
  }
}

//**************програмный сброс*******************
void reset()
{
  wdt_disable();  //отключить, так как он может быть уже включен
  wdt_enable(WDTO_15MS); //установим минимально возможное время 15мс
  while (1) {} //зациклить
}