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