Наши задачки по робототехнике


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


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

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

// пусть роботу нужно проехать 2 м за 35 секунд
R = 2000
T = 35
pi = 3.14
// Размер колеса робота, мм
W = 56
// Рассчитываем, сколько градусов всего нужно проехать, с учетом размера колес
S = R * 360 / (W * pi)
// Рассчитываем среднюю скорость в град/сек
С = S / T
// Мощность на моторах, с которой робот проезжает заданное расстояние за +/- приблизительное заданное время
P = 50
// начинаем движение
Моторы(P)
// Коэф-т ускорения
K = 2
ЦИКЛ
  {
  E1 = считываем показание энкодера моторов
  Ждать (1 сек)
  E2 = считываем показание энкодера моторов   
  // смотрим сколько проехал робот за секунду
  E = E2 - E1  
  // корректируем мощность на моторах
  ЕСЛИ (E > C) ТОГДА P = P - K
  ЕСЛИ (E < C) ТОГДА P = P + K
  Моторы(P)
  // корректируем среднюю скорость с учетом оставшегося расстояния, осталось проехать:
  S = R * 360 / (W * pi) - E2
  // обновление средней скорости
  C = S / (T - таймер)  
  }

Второй вариант решения, с использованием ПИД-регулятора:

// пусть роботу нужно проехать 2 м за 35 секунд
R = 2000
T = 35
pi = 3.14
// Размер колеса робота, мм
W = 56
// Рассчитываем, сколько мм он должен проезжать за 0.1 сек 
S = R / (T * 10)
// Теперь в градусах оборотов мотора c учетом размера колес
S = ((R / (T * 10)) * 360 / (W * pi)
// Мощность на моторах, с которой робот проезжает заданное расстояние за +/- приблизительное заданное время
P = 50

// Коэфт-ты ПИД регулятора
Kp = 0.1
Ki = 0.0001
Kd = 0.2

// начальное значение ошибки регулятора
E = 0

ЦИКЛ
  {
  // предыдущее значение ошибки
  Eold = E
  E1 = считываем показание энкодера моторов
  Ждать (0.1 сек)
  E2 = считываем показание энкодера моторов
  ?/ вычисляем ошибку - на сколько градусов 
  // быстрее или медленнее у робота получилось
  E = (E2-E1) - S
  // сумма всех ошибок - интеграл
  I = I + E
  // ПИД-регулятор
  V = (Kp * E) + (Ki * I) + (Kd * (E - Eold))
  // подаем мощность M на моторы 
  M = P - V
  Моторы(М)
  }


Давайте подумаем: что отличает короткое нажатие от длинного? Правильно - время, в течении которого удерживается нажатой кнопка. Как только кнопка отпускается будем считать что нажатие произошло и мы ждем следующего.

Раз уж мы начали изучать EV3 Basic, давайте решим вторую задачу с его помощью. Алгоритм легко перенести на любой язык программирования.

' Устанавливаем режим работы датчика касания
Sensor.SetMode(1,0)

' Переменные K и D - количество длинных и коротких нажатий
K = 0
D = 0

' Далее начинаем бесконечный цикл
While "True"
  ' Ждем пока датчик не будет нажат
  While Sensor.ReadPercent(1) = 0
  EndWhile

  ' Запоминаем в переменной А время когда нажали кнопку
  A = EV3.Time

  'Ждем пока датчик не отпустят
  While Sensor.ReadPercent(1) = 100
  EndWhile

  ' Запоминаем в переменной В время отпускания кнопки
  B = EV3.Time

  ' Вычисляем время удерживания кнопки нажатой
  X = B - A
  
  ' Если удерживаали долее 500мс - нажатие длинное
  If X > 500 Then
    D = D + 1
  Else
    K = K + 1
  EndIf

  ' Выводим данные на экран
  LCD.Clear()
  LCD.Write(0, 0, "short:")
  LCD.Write(0, 32, K)
  LCD.Write(0, 64, "long:")
  LCD.Write(0, 96, D)

EndWhile




Решение задачи на языке NXC:

float x,d,speed,es,D1,D2,e,u,PD,Pk;

task main()
{
  // инициализация датчиков
  SetSensorLight(IN_2);
  SetSensorLight(IN_3);
  SetSensorLowspeed(IN_4);
  // размер колеи
  x = 180;
  // диаметр колес
  d = 43.2;
  // средняя скорость
  speed = 30;
  // "старая" ошибка Д-регулятора
  es = 0;
  // пропорциональный коэф-т
  Pk = 2;
  // дифференциальный коэф-т
  PD = 4;

  // сбрасываем энкодеры  
  ResetRotationCount(OUT_BС);

  while(true)
  {
    // движемся по линии пока не обнаружим препятствие
    while(SensorUS(IN_4) > 28)
    {
      // показания датчиков линии
      D1 = Sensor(IN_2);
      D2 = Sensor(IN_3);
      // ошибка регулятора
      e = D2-D1;
      // управляющее воздействие
      u = e*Pk+PD*(e-es);
      // подаем его на моторы
      OnFwd(OUT_B,u+speed);
      OnFwd(OUT_C,-u+speed);
      // сохраняем ошибка для Д-регулятора
      es = e;
    }
    // моторы - стоп
    Off(OUT_BC);
    // поворот в сторону внутреннего изгиба трассы
    if (MotorRotationCount(OUT_B)>MotorRotationCount(OUT_C))
    {
      RotateMotorEx(OUT_BC, speed, x/d*52, -100, true, true);
    }
    else
    {
      RotateMotorEx(OUT_BC, speed, x/d*52, 100, true, true);
    }
    Wait(500);
    // съезжаем с линии
    RotateMotorEx(OUT_BC, speed, 1000, 0, true, true);
    // поворот в сторону линии
    if (MotorRotationCount(OUT_B)<MotorRotationCount(OUT_C))
    {
      RotateMotorEx(OUT_BC, speed, x/d*52, -100, true, true);
      OnRev(OUT_B,-18);
      OnRev(OUT_C,-15);
    }
    else
    {
      RotateMotorEx(OUT_BC, speed, x/d*52, 100, true, true);
      OnRev(OUT_B,-15);
      OnRev(OUT_C,-18);
    }
    Wait(500);
    
    // едем до линии
    while(Sensor(IN_3) >= 46 && Sensor(IN_2) >= 44)
    {
      if (Sensor(IN_3)<=46)
      {
        Off(OUT_C);
      }
      if(Sensor(IN_2)<=44)
      {
        Off(OUT_B);
      }
    }
    Off(OUT_BC);
    Wait(1000);
    // пересекаем линию
    RotateMotorEx(OUT_BC, 25, 200, 0, true, true);
    Wait(1000);
    // разворот для продолжения движения по линии
    if (MotorRotationCount(OUT_B)>MotorRotationCount(OUT_C))
    {
      RotateMotorEx(OUT_BC, 25, x/d*90, -100, true, true);
    }
    else
    {
      RotateMotorEx(OUT_BC, 25, x/d*30, 100, true, true);
    }
  }
}


float speed,es,D1,D2,e,u,PD,Pk,speednew1,speednew2;
long time1,time2,time3;
float es1,PD1,PD2,D,es2,e2,e1,u1,u2,Pk1,Pk2;x,d;y;
bool t,r;
int robot;

task main()
{
  // инициализация дальномера
  SetSensorLowspeed(IN_4);
  // направление синхронного разворота
  r = true;
  // номер робота 1 или 2
  robot = 1;
  // у нас в роботах разные датчики, поэтому разная инициализация
  if(robot == 2)
  {
    SetSensorLight(IN_2);
    SetSensorLight(IN_3);
    // один робот будет убегать
    t = true;
  }
  else
  {
    SetSensorColorRed(IN_2);
    SetSensorColorRed(IN_3);
    // другой робот будет догонять
    t = false;
  }

  while(true)
  {
    // размер колеи
    x = 180;
    // диаметр колес, мм
    d = 43.2;

    if (t == true)
    {
      // таймер для синхронного разворота
      time2 = CurrentTick();
      // таймер для периодической смены скорости
      time1 = CurrentTick();
      // первые 2 секунды робот равняется, вперед не едет
      speed = 0;
      // "старая" ошибка Д-регулятора
      es = 0;
      // пропорциональный коэф-т
      Pk = 2;
      // дифференциальный коэф-т
      PD = 4;
      // таймер для 2-секундного выравнивания перед началом движения 
      time3 = CurrentTick();
      // запоминаем, было ли уже выравнивание
      y = 0;
      // фактическая средняя скорость      
      speednew1 = 0;

      while(true)
      {
        // через две секунды начинаем движение после выравнивания
        if(CurrentTick() > time3+2000 && y == 0)
        {
          speednew1 = speed + Random(80);
          y = 1;
        }

        // смена ролей между роботами каждые 29 секунд
        if(CurrentTick() > time2+29000)
        {
          break;
        }
        
        // каждые 5 секунд смена скорости
        if(CurrentTick() > time1+5000)
        {
          speednew1 = speed + Random(80);
          time1 = CurrentTick();
        }

        // показания датчиков
        D1 = Sensor(IN_3);
        D2 = Sensor(IN_2);
        // ошибка регулятора
        e = D2-D1;
        // управляющее воздействие ПД-регулятора
        u = e*Pk+PD*(e-es);
        // подаем его на моторы
        OnFwd(OUT_B,u+speednew1);
        OnFwd(OUT_C,-u+speednew1);
        // запоминаем ошибку для Д-регулятора
        es = e;
      }
      // моторы - стоп
      Off(OUT_BC);
      Wait(1000);
      // смена ролей - разворот
      if (r == true) RotateMotorEx(OUT_BC, 30, x/d*150, 100, true, true);
      else RotateMotorEx(OUT_BC, 30, x/d*150, -100, true, true);
      Wait(1000);
      t = !t;
      r = !r;
    }
    else
    {
      time2 = CurrentTick();
      time3 = CurrentTick();
      speed = 0;
      es1 = 0;
      PD1 = 0;
      Pk1 = 2.5;
      es2 = 0;
      Pk2 = 2;
      PD2 = 6;
      y = 0;

      while(true)
      {
        if(CurrentTick()> time3+2000 && y == 0)
        {
          speed = 50;
          y = 1;
        }
        if(CurrentTick() > time2+29000)
        {
          break;
        }

        D = SensorUS(IN_4);
        D2 = Sensor(IN_2);
        D1 = Sensor(IN_3);
        
        // ограничиваем зону видимости робота - 60 см
        if (D > 60)
        {
          D = 60;
        }

        // здесь у нас целых два регулятора
        // один ведет по линии
        // второй - подстраивает скорость движения
        e2 = D2 - D1;
        e1 = D - 40;
        u2 = e2*Pk2+PD2*(e2-es2);
        u1 = e1*Pk1+PD1*(e1-es1);
        OnFwd(OUT_B,speed+u1+u2);
        OnFwd(OUT_C,speed+u1-u2);
        
        es1 = e1;
        es2 = e2;
      }
      Off(OUT_BC);
      Wait(1000);
      // смена ролей - разворот
      if (r == true) RotateMotorEx(OUT_BC, 30, x/d*150, 100, true, true);
      else RotateMotorEx(OUT_BC, 30, x/d*150, -100, true, true);
      Wait(1000);
      t = !t;
      r = !r;
    }
  }
}

На видео можно посмотреть как роботы решают задачи 5 и 6:



Детская заводная игрушка РОБОТ, выпуска 70-80гг. решала нашу восьмую задачу без всякого сложного программирования и даже не обладая никакой электроникой.



Но так как мы с вами учимся "робототехнировать" %),  для решения задачи нам потребуется собрать 3 моторного робота - два мотора для движения и мотор-"заводной ключ" для "завода пружины". Поворачивая вал третьего "мотора с ключом" мы будем заводить воображаемую пружину.

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

Алгоритм решения данной задачи приведен ниже. 

// Робот начнет движение, если его "завести"
// и не трогать 1 секунду
// Переменная С равна 1 пока мы "заводим" робота
С = 1

ЦИКЛ_ПОКА (C равно 1)
  {
  // В переменные E1 и E2 записываем показания
  // с датчика оборотов с разницей в 1 секунду
  E1 = ДАТЧИК_ОБОРОТОВ(В)
  ЖДАТЬ (1 сек)
  E2 = ДАТЧИК_ОБОРОТОВ(В)
  ЕСЛИ (E2 равно E1) 
    {
    // Робота перестали "заводить", выходии из цикла
    С = 0
    }
  }

// запускаем моторы B и C на E2 градусов вперед
МОТОРы (ВС, E2)





Роботы, как и люди, постоянно ищут что-то, причем делают это своими органами чувств - датчиками -  ультразвуковым, инфракрасным, датчиком освещенности. В нашей 11-й задаче роботу нужно найти объект и развернуться точно на его центр.

Давайте попробуем решить эту задачу. Как обычно мы ищем, например, банки в круге для простейшего кегельринга? Робот разворачивается на месте и как только заметил датчиком расстояния объект, ближе чем указанная граница - останавливается. Причем робот видит край объекта, а инерция доворачивает его до центра банки. Подбирая скорсоть разворота мы отлаживаем направление на центр объекта.

В случае, еесли объект более широкий, чем банка (например ящик), такой способ будет работать криво. Что же делать? Нужно вычислить угловые размеры объекта, ориентируясь на момент, когда робот нашел объект и момент, когда объект потерян.



Предположим поиск идет по часовой стрелке. В процессе разворота отмечаем доступным способом (с помощью энкодера мотора или хотя бы таймером) метку левого края объекта, затем правого. Находим между ними середину и возвращаемся на центр объекта движением против часовой стрелки на половину угла между метками.

Пусть моторы подключены к портам В и С. На псевдокоде программа c использованием энкодера будет выглядеть следующим образом:

МОТОРЫ (50,-50)
ЖДИ (РАССТОЯНИЕ < 50)
А = ЭНКОДЕР (В)
ЖДИ (РАССТОЯНИЕ > 50)
B = ЭНКОДЕР (В)
МОТОРЫ (СТОП)
С = (B -A) / 2
МОТОРЫ_НА)ГРАДУСЫ(-50,50,С)

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

A = ТАЙМЕР (1)
МОТОРЫ (50,-50)
ЖДИ (РАССТОЯНИЕ < 50)
B = ТАЙМЕР (1)
ЖДИ (РАССТОЯНИЕ > 50)
C = ТАЙМЕР (1)
МОТОРЫ (СТОП)
МОТОРЫ(50,-50)
ЖДИ (С-A)
МОТОРЫ (СТОП)
D = (B - A) + (C -B) / 2
МОТОРЫ (50,-50)
ЖДИ (D)
МОТОРЫ (СТОП)

Очевидно, что данный способ применим не только для поиска объектов датчиков расстояния. Используя датчик освещенности робот может "нащупать" центр широкой линии, а используя датчик цвета - середину цветной поверхности.



Наш робот будет искать свое место под Солнцем. Как известно, роботы лучше всего себя чувствуют в теплых и сухих местах, а значит Солнце светит там особенно ярко.Оборудуем нашего робота датчиком освещенности, направленным вверх и настроенным в режим измерения внешней освещенности.

Решение задачи не предполагает построения карты освещенности, оно будет основано на методе "проб и ошибок". Наш робот будет ехать вперед на длину своего корпуса и сравнивать, не стала ли в этом месте освещенность выше, чем там, где он стоял до этого. Если стала - он движется в нужном направлении и следует продолжить путь. Если нет - следует вернуться назад и придумать, куда бы ему двинуться дальше и попытать счастья развернувшись для поиска направо или налево.

На псевдокоде решение задачи выглядит так:

// L - Освещенность (яркость внешнего освещения)
L = ДАТЧИК(ОСВЕЩЕННОСТЬ)
// T - смена направления поиска
T = ИСТИНА
ЦИКЛ (ВСЕГДА)
  ЕСЛИ (T = ИСТИНА)
    // ВЫБИРАЕМ НАПРАВЛЕНИЕ ПОВОРОТА
    N = СЛУЧАЙНОЕ_ЧИСЛО (2)
    ЕСЛИ (N = 1) МОТОРЫ(ПОВОРОТ_ВЛЕВО)
    ЕСЛИ (N = 2) МОТОРЫ(ПОВОРОТ_ВПРАВО)
  // ИЗМЕРЯЕМ ОСВЕЩЕННОСТЬ В НОВОЙ ОБЛАСТИ
  МОТОРЫ (ВПЕРЕД_НА_ДЛИНУ_РОБОТА)
  МОТОРЫ (СТОП)
  ЕСЛИ (ДАТЧИК(ОСВЕЩЕННОСТЬ) > L)
    L = ДАТЧИК(ОСВЕЩЕННОСТЬ)
    T = ЛОЖЬ
  ИНАЧЕ
    МОТОРЫ (НАЗАД_НА_ДЛИНУ_РОБОТА)
    МОТОРЫ (СТОП)
    Т = ИСТИНА
КОНЕЦ_ЦИКЛА
Усложненный кегельринг. Робот должен выбить все кегли за пределы круга, при этом подвижный флажок, установленный на роботе должен всегда смотреть в направлении старта робота.

Код программы  на EV3 Basic:

Sensor.SetMode(2,0)
Sensor.SetMode(2,1)
Sensor.SetMode(2,0)
Sensor.SetMode(1,0)
Sensor.SetMode(3,0)
Sensor.SetMode(4,0)

Thread.Run = FLAG

S = 15

Sub FLAG
  Motor.ResetCount("A")
  Pk = 0.15
  Kk = 0.0001
  While "True"
    Er = Sensor.ReadRawValue(2,0) + Motor.GetCount("A")
    V = Er*Pk + Er*Er*Er*Kk 
    Motor.Start("A",-V)
  EndWhile
EndSub

While Sensor.ReadPercent(4) < 50
  Program.Delay(1)
EndWhile
While "True"
  Motor.StartSync("BC", S, -S)
  If Sensor.ReadPercent(1) < 63 Then
    Motor.Stop("BC","True") 
    Motor.ResetCount("BC")
    Motor.StartSync("BC", 50, 50)
    While Sensor.ReadPercent(3) > 25
      Program.Delay(1)
    EndWhile
    Motor.Stop("BC","True")    
    Motor.MoveSync("BC", -50, -50, Motor.GetCount("C"), "True")
    Motor.Stop("BC","True")    
  EndIf

EndWhile 


Движение по "восьмерке", используя только датчик-гироскоп.

Код программы на EV3 Basic:

Sensor.SetMode(2,0)
Sensor.SetMode(2,1)
Sensor.SetMode(2,0)
Sensor.SetMode(2,1)
Sensor.SetMode(2,0)

Napr = 0
Er = 0
Pk = 1.8
Kk = 0.003
Sp = 20

Thread.Run = NAPRAVLENIE

Sub NAPRAVLENIE
  While "True"
    
    For Napr = 0 To 360
      Program.Delay(30)
    EndFor
    
    For Napr = 360 To 0 Step -1
      Program.Delay(30)
    EndFor
    
  EndWhile
EndSub

While "True"
  Er = Napr - Sensor.ReadRawValue(2,0)
  V = Er * Pk + Er * Er * Er * Kk
  Motor.StartSync("BC", Sp-V, Sp+V)
EndWhile

Программа для решения задачи на языке NXC:

// Создаем переменные
float x,d;
int speed,es,D1,D2,e,u,PD,Pk;

task main()
{
  // Инициализируем датчики
  SetSensorLight(IN_2); // Датчик освещенности слева 2й порт
  SetSensorLight(IN_3); // Датчик освещенности справа 3й порт
  SetSensorLowspeed(IN_4); // Ультразвуковой датчик - 4й порт

  x = 180; // Ширина колеи
  d = 43.2; // Диаметр колес робота
  speed = 70; // Средняя скорость 
  es = 0; // "старая" ошибка ПД - регулятора
  Pk = 2; // Пропорциональный коэф-т ПД-регулятора
  PD = 4; // Дифференциальный коэф-т ПД-регулятора
  
  OnFwdSync(OUT_BC, 20, -100); // разворачиваемся в поисках банки
  while(SensorUS(IN_4) >= 50); // ожидаем банку
  while (SensorUS(IN_4) <= 50); // ожидаем потерю банки
  Off(OUT_BC); // останавливаем моторы
  // разворачиваемся для выезда рядом с банкой
  RotateMotorEx(OUT_BC, speed, x/d*85, -  100, true, true);
  Wait(50);
  OnFwdSyncEx(OUT_BC, 50, 0,RESET_NONE);
  // едем вперед пока не обнаружена линия
  while(Sensor(IN_3) >= 44 && Sensor(IN_2) >= 40.5);
  Off(OUT_BC); // останавливаемся
  Wait(50);
  // немного вперед, чтобы развернуться над линией
  RotateMotorEx( OUT_BC , speed , 200 , 0 , true , true );
  // поворачиваем вправо, УЗ-датчик смотрит в круг
  RotateMotorEx(OUT_BC , speed , x / d * 75, -100, true , true );
  // цикл для 4 банок
  for(int i=0; i<4; i++)
  {
    // пока нет банки - едем по линии на ПД-регуляторе
    while(SensorUS(IN_4) >=10)
    {
      // показания с датчиков
      D1 = Sensor(IN_3);
      D2 = Sensor(IN_2);
      // ошибка регулятора - разница показаний
      e = D2-D1;
      // управляющее воздействие по формуле ПД-регулятора
      u = e*Pk+PD*(e-es);
      // рассчитанную мощность подаем на моторы
      OnFwd(OUT_B,u+speed);
      OnFwd(OUT_C,-u+speed);
      // сохраняем ошибку для следующей итерации цикла
      es = e;
    }
    // останавливаемся при обнаружении банки
    Off(OUT_BC);
    // поворачиваемся к банке
    RotateMotorEx(OUT_BC , speed , x / d * 95, -100, true , true );
    Wait(50);
    // заталкиваем банку в центр круга
    RotateMotorEx( OUT_BC , speed , 1100 , 0 , true , true );
    Wait(50);
    // возвращаемся на линию
    RotateMotorEx( OUT_BC , -1*speed , 1100 , 0 , true , true );
    // разворачиваемся влево, чтобы продолжить движение
    RotateMotorEx(OUT_BC , speed , x / d * 87, 100, true , true );
  }
}




// создаем много переменных
int B_enkoder,C_enkoder,D1,D2,trigger;
bool out;
int e,u,Pk,PD,speed,es;
float d,x;

task main()
{
  // инициализируем датчики освещенности
  SetSensorLight(IN_2); 
  SetSensorLight(IN_3);

  x = 180; // ширина колеи
  d = 43.2; // Диаметр колес робота
  Pk = 2; // Пропорциональный коэф-т ПД-регулятора
  PD = 6; // Дифференциальный коэф-т ПД-регулятора
  speed = 75; // Средняя скорость 

  while(true)
  {
    // сбрасываем показания энкодеров
    ResetRotationCount(OUT_B);
    ResetRotationCount(OUT_C);
    // сохраняем показания энкодеров
    B_enkoder = 0;
    C_enkoder = 0;    
    es = 0; // "старая" ошибка ПД - регулятора
    out = true; // по изменению этой переменной будем вылетать из цикла
    Wait(10);
    while(out)
    {
      // показания с датчиков
      D1 = Sensor(IN_3);
      D2 = Sensor(IN_2);
      // ошибка регулятора - разница показаний
      e = D2-D1;
      // управляющее воздействие по формуле ПД-регулятора
      u = e*Pk+PD*(e-es);
      // рассчитанную мощность подаем на моторы
      OnFwd(OUT_B,u+speed);
      OnFwd(OUT_C,-u+speed);
      // сохраняем ошибку для следующей итерации
      es = e;
      // если проехали половину круга - вылетаем из цикла
      if ((MotorRotationCount(OUT_B)+MotorRotationCount(OUT_C))/2*(d*PI/360) >= (1070*PI)/2)
      {
        out = false;
      }
    }
    // стоп моторы
    Off(OUT_BC);
    // это пауза 50 мс
    Wait(50);
    
    // смотрим на каком энкодере набралось больше показаний
    // в зависимости от этого разворачиваемся к соответствующую
    // сторону и сохраняем в trigger
    if (MotorRotationCount(OUT_B)-B_enkoder>MotorRotationCount(OUT_C)-C_enkoder)
    {
      RotateMotorEx(OUT_BC, speed, x/d*90, -100, true, true);
      trigger = 1;
    }
    else
    {
      RotateMotorEx(OUT_BC, speed, x/d*90, 100, true, true);
      trigger = 2;
    }
    // выравниваемся о линию     OnRev(OUT_B,15);
    OnRev(OUT_C,15);
    while(Sensor(IN_3) >= 46 && Sensor(IN_2) >= 44)
    {
      if (Sensor(IN_3)<=44)
      {
        Off(OUT_C);
      }
      if(Sensor(IN_2)<=40.5)
      {
        Off(OUT_B);
      }
    }
    Off(OUT_BC);
    Wait(50);
    // едем через круг
    OnFwdSyncEx(OUT_BC, speed, 0,RESET_NONE);
    while(Sensor(IN_3) >= 44 && Sensor(IN_2) >= 40.5)
    {
    }
    // при обнаружении линии - останавливаемся
    Off(OUT_BC);
    Wait(50);
    // немного вперед, чтобы развернуться над линией
    RotateMotorEx(OUT_BC, speed, 200, 0, true, true);
    // в trigger - направление разворота
    if (trigger == 1)
    {
      RotateMotorEx(OUT_BC, speed, x/d*90, 100, true, true);
    }
    else
    {
      RotateMotorEx(OUT_BC, speed, x/d*90, -100, true, true);
    }
    // сохраняем показания энкодеров
    B_enkoder = MotorRotationCount(OUT_B);
    C_enkoder = MotorRotationCount(OUT_C);
  }
}




Комментариев нет:

Отправить комментарий

Самое популярное