суббота, 23 июля 2016 г.

EV3 Параллельная парковка

В этом сезоне в одной из категорий Всероссийской Робототехнической Олимпиады "Роботраффик" нам запомнилась интересная задачка, "параллельная парковка". Мы подумали что было бы интересно решить ее с использованием платформы LEGO Mindstorms EV3.



Техническое задание звучало так: создать автономное роботизированное транспортное средство, способное самостоятельно провести процедуру парковки в случае наличия свободного места в зоне парковки. 



Двигаясь вдоль парковочной зоны ТС должно определить место, достаточное для парковки, и занять это место.


Для решения задачи мы решили воспользоваться распространенным робо-реквизитом: кругом для сумо (кегельринга) в качестве трассы, кеглями и банками - в качестве макетов ТС, стоящих на обочине.
Автономное роботизированное транспортное средство, которое мы собрали для решения данной задачи основано на шасси от бонусной модели RAC3 TRUCK из домашней версии EV3. Мы доработали рулевое управление модели для достижения больших углов разворота и установили датчики - пару датчиков освещенности для движения по линии и ИК-датчик расстояния для определения макетов транспортных средств на обочине.


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

Моторы и датчики подключены так:
  • Мотор А - рулевое управление, моторы B и С - ходовые
  • Порты 1 и 2 - датчики линии, порт 4 - ИК-дальномер
Программу для робота мы написали в среде EV3 Basic. Выглядит она следующим образом:

' инициализируем датчики
Sensor.SetMode(1,0)
Sensor.SetMode(2,0)
Sensor.SetMode(4,0)

' переменные - коэф-ты для регулятора
k = 2
Pk = 7
Dk = 10  
Popr = 12
Ug = 0
M = "A"
e_o = 0

' триггер для подсчета банок
trigger = "False"

' запускаем ходовые моторы
Motor.Start("B",-75)
Motor.Start("C",-75)

While "True"
  While "True"
    ' разница в показаниях датчиков с учетом поправки  
    r = Sensor.ReadPercent(1)-(Sensor.ReadPercent(2)+Popr)
    ' требуемый угол разворота колес
    Ug = r * k
    ' ошибка регулятора - разница требуемого угла и текущего
    e = Ug - Motor.GetCount("A")
    ' подаем управляющее воздействие на рулевой мотор  
    Motor.Start(M,e*Pk+Dk*(e-e_o))
    ' сохраняем ошибку для работы дифференциальной компоненты регулятора
    e_o = e
    
    ' определяем расстояние ИК-датчиком
    tmp = Sensor.ReadPercent(4)
    
    If tmp < 22 Then
      ' если банка "первая"
      If trigger = "False" Then
        ' сбрасываем энкодеры и запоминаем в триггере
        Motor.ResetCount("BC")
        trigger = "True"
      Else
        ' иначе проверяем - умещается ли ТС в промежутке
        if (Motor.GetCount("B") + Motor.GetCount("C")) / 2 < -979 Then
          ' если да - паркуемся
          Goto parkovka
          trigger = "False"
        Else
          ' если нет - считаем эту банку "первой"
          Motor.ResetCount("BC")
          trigger = "True"
        EndIf
      EndIf
    EndIf
       
  EndWhile    
  
  ' маневр парковки
  parkovka:
  
  Speaker.Note(100,"C4",500)
  
  Motor.ResetCount("BC")
  
  ' проезжаем вперед по линии "500 градусов"
  While (Motor.GetCount("B") + Motor.GetCount("C"))/2 > -500
    r = Sensor.ReadPercent(1)-(Sensor.ReadPercent(2)+Popr)
    Ug = r * k
    e = Ug - Motor.GetCount("A")  
    Motor.Start(M,e*Pk+Dk*(e-e_o))
    e_o = e
  EndWhile
  
  ' маневр заезда на парковочное место
  Motor.Schedule("A",30,0,90,0,"True")
  Motor.Schedule("BC",30,0,530,0,"True")
  Motor.Wait("ABC")
  Motor.Schedule("A",-30,0,115,0,"True")
  Motor.Schedule("BC",30,0,950,0,"True")
  Motor.Wait("ABC")
  
  Motor.Stop("BC","True")
  Program.Delay(1000)
  
  ' маневр выезда из парковочного места
  Motor.Schedule("A",50,0,15,0,"False")
  Motor.Schedule("BC",-30,0,730,0,"True")
  Motor.Wait("ABC")
  Motor.Schedule("A",30,0,65,0,"True")
  Motor.Schedule("BC",-30,0,250,0,"True")
  Motor.Wait("ABC")
  
  Motor.Start("B",-75)
  Motor.Start("C",-75)

EndWhile

7 комментариев:

  1. А почему бы вместо второго While "True" не сделать что-то вроде:
    While (Motor.GetCount("B") + Motor.GetCount("C")) / 2 < -979
    и парковаться сразу, как только нашли достаточный просвет (не дожидаясь следующей банки/кегли/машины)?

    ОтветитьУдалить
    Ответы
    1. Павел, это имеет смысл, согласны. Однако мы специально хотели уйти от случая парковки в "чистом поле", чтобы робот парковался именно между банками/кеглями, а не в любом достаточном по габаритам пространстве.

      Удалить
    2. Идею понял, хотя, применительно к Роботрафику, мне кажется, лучше парковаться сразу (при первой же возможности) для экономии времени.

      И еще вопрос, если можно (давно я не работал с Бейсиком):
      Goto parkovka
      trigger = "False"
      goto - это же безусловный переход, а не вызов подпрограммы, разве команда после goto (trigger = "False") выполнится?

      Удалить
    3. Павел, действительно, интересная конструкция - спросил детей - они сделали это "для надежности", чтоб уж точно выпасть из цикла :)

      Удалить
  2. не могу понять, а где инструкция сборки? Или тут нужно самому конструкцию придумать на основе того бонусного робота?

    ОтветитьУдалить
    Ответы
    1. Этот комментарий был удален автором.

      Удалить
    2. Ссылка на инструкцию и программу в тексте статьи. Вот она - https://yadi.sk/d/u_CkI1GAtf53e Инструкция в формате LEGO Digital Designer, файл parkovka.lxf

      Удалить

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