суббота, 31 декабря 2016 г.

С Новым Годом!

Я, Большой железный робот и мои создатели, команда робототехников Карандаш и Самоделкин, поздравляем всех с наступающим Новым годом!
Хочется всем пожелать, чтобы в наступающем году с нами произошло то самое чудо, о котором мы все так мечтаем. Хотя у каждого оно свое, но оно обязательно самое необходимое и самое важное. Желаю никому не ржаветь, иметь под рукой свежую батарейку, на которую можно всегда рассчитывать. Желаю Вам в грядущем году быть в окружении исключительно положительных роботов и доброжелательных людей, переживать только приятные эмоции без багов, радоваться каждому прожитому циклу, дарить радость и энергию окружающим. Пусть этот Новый год станет для Вас особенным. С Новым Годом!


суббота, 3 декабря 2016 г.

Решение задач №5 и 6

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



В первой задаче наш робот будет объезжать препятствия на линии и возвращаться на нее для продолжения движения. 
Решение задачи на языке 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;
    }
  }
}

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



воскресенье, 27 ноября 2016 г.

NAVIDOZ3R: инерциальная навигация


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


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


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

Данная задача - на инерциальную навигацию. Что это такое? Как гласит Википедия, инерциальная навигация — метод навигации (определения координат и параметров движения различных объектов — судов, самолётов, ракет и др.) и управления их движением, основанный на свойствах инерции тел, являющийся автономным, то есть не требующим наличия внешних ориентиров или поступающих извне сигналов. Неавтономные методы решения задач навигации основываются на использовании внешних ориентиров или сигналов (например, звёзд, маяков, радиосигналов и т. п.). Эти методы в принципе достаточно просты, но в ряде случаев не могут быть осуществлены из-за отсутствия видимости или наличия помех для радиосигналов и т.п. Необходимость создания автономных навигационных систем явилась причиной возникновения инерциальной навигации.


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

В этот раз мы не будем выдумывать нового робота, а совместим "приятное с полезным" - соберем одну из ранее не собиравшихся нами бонусных моделей, инструкция по сборке к которой свободно доступна всем желающим в составе домашней версии ПО LEGO.
Немного погоняв собранного ROBODOZ3R, мы приняли решение устранить недостатки в его шасси. Как показали испытания, шестеренки, установленные в качестве верхних колес, недостаточно уверенно натягивают гусеницы. Вместо них мы рекомендуем использовать третью пару колесных дисков. Эта модификация заметно сказывается на точности движений робота, гусеницы перестают проскальзывать.


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


С конструкцией разобрались, теперь давайте поставим роботу задачу, которую ему предстоит решить. Вернемся к нашему роботу на планете и спутнику. Спутник в нашем проекте - виртуальный, в его роли выступает ПК. На экране компьютера оператор видит заранее нарисованную "карту местности", на которой предстоит работать роботу. Мышкой по карте устанавливаются стартовая позиция робота и последовательность путевых точек, которые роботу предстоит пройти. Далее по нажатию кнопки "Сохранить маршрут" эти данные сохраняются в файл на работе.


Программировать мы снова будем на EV3 Basic. Задумав этот проект, мы хотели в том числе пощупать его возможности по гибридному режиму работы "робот-ПК", когда часть кода исполняется на ПК, часть кода на роботе. При этом робот должен быть соединен с компьютером по USB, Bluetooth или Wi-Fi.

Давайте посмотрим на первую, гибридную программу, которая выполняется и на ПК и на роботе:

' Массивы с путевыми точками
PTx[0] = 0
PTy[0] = 0

' Показываем графическое окно
GraphicsWindow.Show()

' Очистить окно
GraphicsWindow.Clear()

' Заголовок окна
GraphicsWindow.Title = "NAVIDOZ3R"

' запрещаем изменять размеры окна
GraphicsWindow.CanResize = "False"

' Размеры окна
GraphicsWindow.Width=825
GraphicsWindow.Height=640

' Окно по центру экрана

GraphicsWindow.Left = (Desktop.Width - GraphicsWindow.Width) / 2
GraphicsWindow.Top = (Desktop.Height - GraphicsWindow.Height) / 2

' Цвет фона
GraphicsWindow.BackgroundColor = "Black"
GraphicsWindow.PenColor = "Red"

' Шрифт
GraphicsWindow.FontName = "Arial"
GraphicsWindow.FontSize = 20
GraphicsWindow.PenColor = "Black"

' фоновая картинка:
background = ImageList.LoadImage(Program.Directory + "/map1.jpg")
GraphicsWindow.DrawImage(background, 0, 0)

' Разрешение карты в пикселях
ImageX_pix = 480
ImageY_piy = 480

' Размеры поля в миллиметрах
ImageX_mm = 1117
ImageY_mm = 1116

' Добавим кнопки
btnPOI = Controls.AddButton("Сохранить маршрут", 20,GraphicsWindow.Height-100)
btnStart = Controls.AddButton("Старт", 520,GraphicsWindow.Height-100)

' Событие кнопки
Controls.ButtonClicked = OnClick

' количество путевых точек
POI = 0

Sub OnClick
  Sound.PlayClick()
  btn=Controls.LastClickedButton
  If btn="Button1" then
    GraphicsWindow.ShowMessage("Маршрут записан в робота","EV3")
    F = EV3File.OpenWrite("/home/root/lms2012/prjs/NAVIDOZ3R/NAVIDOZ3R_PT.txt")
    EV3File.WriteLine(F,POI)
    For i = 0 To POI-1
      EV3File.WriteLine(F,Math.Round(PTx[i]))
      EV3File.WriteLine(F,Math.Round(ImageY_mm - PTy[i]))
    EndFor    
    EV3File.Close(F)
  Else
    ' выход из программы
    Program.End()
  EndIf
EndSub

' Отслеживание событий нажатия кнопки мыши
GraphicsWindow.MouseDown = OnMouseDown

Sub OnMouseDown
  Sound.PlayClick()
  If POI = 0 Then
    GraphicsWindow.BrushColor = "Blue"
  Else  
    GraphicsWindow.BrushColor = "Red"
  EndIf
  GraphicsWindow.FillEllipse(x-5,y-5,10,10)
  Program.Delay(500)
  
  PTx[POI] = x*ImageX_mm/ImageX_pix
  PTy[POI] = y*ImageY_mm/ImageY_piy
  
  If POI >= 1 Then
     GraphicsWindow.DrawLine(PTx[POI-1]/ImageX_mm*ImageX_pix, PTy[POI-1]/ImageY_mm*ImageY_piy, PTx[POI]/ImageX_mm*ImageX_pix, PTy[POI]/ImageY_mm*ImageY_piy)
  EndIf
  POI = POI + 1  
EndSub

While "True"
  x = GraphicsWindow.MouseX
  y = GraphicsWindow.MouseY
EndWhile

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


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

Program.Delay(2000)
Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/load")
Speaker.Wait()
Program.Delay(1000)

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

' Предварительная попытка устранения дрифта
Sensor.SetMode(2,0)

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

Гироскоп EV3 подвержен неприятному явлению - дрейфу показаний (дрифту), поэтому оцениваем его в течении 5 секунд, для дальнейшей компенсации.

Time = EV3.Time
' Взятие дрифта за 5 секунд
Gyro1 = Sensor.ReadRawValue(3,0)
Program.Delay(5000)
Gyro2 = Sensor.ReadRawValue(3,0)
Drift10 = (Gyro1 - Gyro2) / 500

Получили в переменной Drift10 величину дрифта за 10 мс. Теперь открываем файл с путевыми точками и определим их количество.

F = EV3File.OpenRead("/home/root/lms2012/prjs/NAVIDOZ3R/NAVIDOZ3R_PT.txt")
Line = EV3File.ConvertToNumber(EV3File.ReadLine(F))
PT = Line

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

LCD.Clear()
LCD.Text(1,0,0,2,Line)

Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/plan")

For i = 0 To PT-1
  Line = EV3File.ConvertToNumber(EV3File.ReadLine(F))
  PTx[i] = Line
  LCD.Text(1,0,i*10+20,1,Line)
  
  Program.Delay(50)
  
  Line = EV3File.ConvertToNumber(EV3File.ReadLine(F))
  PTy[i] = Line
  LCD.Text(1,40,i*10+20,1,Line)
  
  Program.Delay(50)
EndFor 
EV3File.Close(F)

Program.Delay(3000)

Создаем кучу переменных, они нам все обязательно понадобятся

' Объявление переменных
u = 0
e = 0            
e_old = 0
Pk = 4
Dk = 8

X = 0
Y = 0
X_old = 0
Y_old = 0
X_new = 0
Y_new = 0
X_tmp = 0
Y_tmp = 0

speed = 25
diam = 32.75125
Dist = 0
Azimut = 0
Azimut_old = 0
Gyro = 0
Finish = "False"
TimeTurn = 0

Далее опишем параллельную задачу (подпроцесс) в котором считываются показания гироскопа и компенсируется дрифт. Итоговые показания публикуем в переменную Gyro. Именно из нее, а не с гироскопа мы будем брать показания.

' Подпроцесс передаёт показания гироскопа с устранением дрифта
Sub Gyro
  While"True"
    Gyro = -1*(Sensor.ReadRawValue(3,0) - Drift10 * (Time/10))
  EndWhile
EndSub

Создадим процедуру, которая выводит на экран текущие координаты робота, предыдущую путевую точку и текущий азимут (направление движения). Эта информация крайне полезна при отладке и анализе текущей ситуации.

Sub Display
  
  Speaker.Note(100,"C5",500)
  
  LCD.Clear()
  
  LCD.Text(1,0,0,2,X)
  LCD.Text(1,0,20,2,Y)
  
  LCD.Text(1,0,40,1,X_old)
  LCD.Text(1,0,50,1,Y_old)
  
  LCD.Text(1,0,70,2,Azimut)
  LCD.Text(1,0,90,1,Gyro)
  
  Program.Delay(3000)
  
EndSub

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

Зная свои координаты (X,Y) и координаты целевой точки (X_new,Y_new) робот должен рассчитать две величины - расстояние, которое от должен проехать Dist и направление, в котором он должен это сделать Azimut. Расстояние Dist рассчитывается просто, по теореме Пифагора:


А направлением Azimut сложнее, в зависимости от направления движения оно рассчитывается по разному:

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

Следующая процедура, которая нам понадобится - Go_Forward - ведет робота вперед ан заданное расстояние Dist, удерживая при этом направление Azimut. Если в процессе движения на пути возникает препятствие, она вызывает одну из процедур объезда Detour1() .. Detour3().
Сбрасываем энкодеры моторов на шасси и используя ПИД-регулятор движемся вперед компенсируя отклонения показаний регулируемой величины Gyro от целевой Azimut. Следует учесть, что Azimut у нас увеличивается по часовой стрелке, а Gyro - против. 
Во время движения обновляем показания текущих координат робота (X,Y).

Sub Go_Forward
  Motor.ResetCount("BC")
  Finish = "True"
  While Math.Abs((Motor.GetCount("B")+Motor.GetCount("C"))/2) < Dist/(diam*Math.Pi)*360
    e = Azimut-(-1)*Gyro
    
    u=Pk*e+Dk*(e-e_old)
    
    Motor.Start("B",-1*speed+u)
    Motor.Start("C",-1*speed-u)
    
    e_old = e
    
    AzimutTMP = Math.Remainder(Azimut+3600,360)
    
    if AzimutTMP < 90 Then
      X_tmp = Math.Abs(Motor.GetCount("B")+Motor.GetCount("C"))/2*(diam*Math.Pi/360)*Math.Sin(Math.GetRadians(Math.Remainder(AzimutTMP+3600,90)))
      Y_tmp = Math.Abs(Motor.GetCount("B")+Motor.GetCount("C"))/2*(diam*Math.Pi/360)*Math.Cos(Math.GetRadians(Math.Remainder(AzimutTMP+3600,90)))
      
      X = X_old+X_tmp
      Y = Y_old+Y_tmp
    ElseIf 90 <= AzimutTMP and AzimutTMP < 180 Then
      X_tmp = Math.Abs(Motor.GetCount("B")+Motor.GetCount("C"))/2*(diam*Math.Pi/360)*Math.Cos(Math.GetRadians(Math.Remainder(AzimutTMP+3600,90)))
      Y_tmp = Math.Abs(Motor.GetCount("B")+Motor.GetCount("C"))/2*(diam*Math.Pi/360)*Math.Sin(Math.GetRadians(Math.Remainder(AzimutTMP+3600,90))) 
      
      X = X_old+X_tmp
      Y = Y_old-Y_tmp
    ElseIf 180 <= AzimutTMP and AzimutTMP < 270 Then
      X_tmp = Math.Abs(Motor.GetCount("B")+Motor.GetCount("C"))/2*(diam*Math.Pi/360)*Math.Sin(Math.GetRadians(Math.Remainder(AzimutTMP+3600,90)))
      Y_tmp = Math.Abs(Motor.GetCount("B")+Motor.GetCount("C"))/2*(diam*Math.Pi/360)*Math.Cos(Math.GetRadians(Math.Remainder(AzimutTMP+3600,90)))
      
      X = X_old-X_tmp
      Y = Y_old-Y_tmp
    ElseIf 270 <= AzimutTMP and AzimutTMP < 360 Then
      X_tmp = Math.Abs(Motor.GetCount("B")+Motor.GetCount("C"))/2*(diam*Math.Pi/360)*Math.Cos(Math.GetRadians(Math.Remainder(AzimutTMP+3600,90)))
      Y_tmp = Math.Abs(Motor.GetCount("B")+Motor.GetCount("C"))/2*(diam*Math.Pi/360)*Math.Sin(Math.GetRadians(Math.Remainder(AzimutTMP+3600,90))) 
      
      X = X_old-X_tmp
      Y = Y_old+Y_tmp
    EndIf 
    
    If Sensor.ReadPercent(2)<62 Then
      Finish = "False"
      Motor.Stop("BC","True")
      Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/break")
      Speaker.Wait()
      Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/objezd")
      Speaker.Wait()
      
      Display()
      'Program.Delay(8000)
      
      X_old = X
      Y_old = Y
      
      Detour3()
      
      X_old = X
      Y_old = Y
      
      Display()
      'Program.Delay(8000)
      
      GotoXY()
      
    EndIf  
    Program.Delay(10)
  EndWhile
  Motor.Stop("BC","True")
  Program.Delay(100)
EndSub

Далее опишем три процедуры объезда препятствий. Первая из них выполняет простой объезд "по половинке ромба":
Sub Detour1
  'R = Sensor.ReadPercent(2)
  
  Azimut_old = Azimut
  Azimut = Azimut+45
  MinimumTurn()
  Turn()
  
  Dist = Math.SquareRoot(22*22+22*22)*10
  Go_Forward()  
  
  Display()
  'Program.Delay(8000)
  
  X_old = X
  Y_old = Y
  
  Azimut_old = Azimut
  Azimut = Azimut-90
  MinimumTurn()
  Turn()
  
  Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/vozvrat")
  Speaker.Wait()
  
  Dist = Math.SquareRoot(22*22+22*22)*10
  Go_Forward()
  Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/continue")
  Speaker.Wait()
EndSub

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


Sub Detour2
  'R = Sensor.ReadPercent(2)
  
  Azimut_old = Azimut
  Azimut = Azimut-90
  MinimumTurn()
  Turn()
  
  Dist = 180
  Go_Forward()  
  
  Display()
  'Program.Delay(8000)
  
  X_old = X
  Y_old = Y
  
  Azimut_old = Azimut
  Azimut = Azimut+90
  MinimumTurn()
  Turn()
  
  Dist = 360
  Go_Forward()
  
  Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/vozvrat")
  Speaker.Wait()
  
  X_old = X
  Y_old = Y
  
  Azimut_old = Azimut
  Azimut = Azimut+90
  MinimumTurn()
  Turn()
  
  Dist = 180
  Go_Forward()
    
  Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/continue")
  Speaker.Wait()
EndSub

Третий вариант объезда - "по половинке правильного многоугольника" также является рекурсивным. При большом количестве сторон получаем объезд по полукругу.

Например процедура для 12-угольника:

Sub Detour3
  
  For h=1 To 6
    
    'Display()
    'Program.Delay(8000)
    
    X_old = X
    Y_old = Y
    
    Azimut_old = Azimut
    If h = 1 Then
      Azimut = Azimut-75
    Else
      Azimut = Azimut+30  
    EndIf  
    MinimumTurn()
    Turn()
    
    Dist = 103.52
    Go_Forward()
    
    If h=5 Then
      Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/vozvrat")
      Speaker.Wait()  
    EndIf
  EndFor
  
  Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/continue")
  Speaker.Wait()
EndSub

Чтобы минимизировать углы разворота при следовании по маршруту потребуется процедура MinimumTurn(). Необходимость ее использования связана с тем, что показания гироскопа могут выходить за пределы 0..360. Она переносит текущий расчетный азимут в тот "круг", в пределах которого находятся сейчас показания гироскопа. Таким образом любой разворот не сможет превысить 180 градусов.

Sub MinimumTurn
  For j=-10 To 10
    If Math.Abs(Azimut+j*360-Azimut_old)<180 Then
      Azimut = Azimut+j*360
    EndIf
  EndFor
EndSub

Процедура Turn() выполняет поворот, ориентируя робота в нужном направлении, причем ей не важно как робот ориентирован в данный момент. Она использует ПИД-регулирование, подобно процедуре Go_forward(), однако рассчитанное результирующее воздействие не суммируется с постоянной величиной скорости, тем самым робот разворачивается на месте.

Sub Turn
  TimeTurn = EV3.Time
  While Math.Abs(Azimut-(-1)*Gyro)>=3 And EV3.Time-TimeTurn<5000
    e = Azimut-(-1)*Gyro
    
    u=Pk*e+Dk*(e-e_old)
    
    Motor.Start("B",u)
    Motor.Start("C",-1*u)
    
    e_old = e
    Program.Delay(10)  
  EndWhile
  Motor.Stop("BC","True")
  Program.Delay(100)
EndSub

Процедура GotoXY() выполняет все цепочку действий, необходимых для достижения следующей путевой точки маршрута:
  • рассчитывает новый Azimut
  • минимизирует угол разворота (выбирая кратчайший маневр) вызывая MinimumTurn()
  • разворачивает робота в заданном направлении вызывая Turn()
  • рассчитывает расстояние до следующей путевой точки Dist
  • перемещает робота, сохраняя направление, вызовом Go_forward() и выполняя объезды препятствий
Sub GotoXY
  Azimut_old = Azimut
  
  If X_old <> X_new And Y_old <> Y_new Then
    
    If X_old>X_new And Y_old>Y_new Or X_old<X_new And Y_old<Y_new Then
      Azimut = Math.GetDegrees(Math.ArcTan(Math.Abs(X_old-X_new)/Math.Abs(Y_old-Y_new)))
    Else
      Azimut = Math.GetDegrees(Math.ArcTan(Math.Abs(Y_old-Y_new)/Math.Abs(X_old-X_new)))  
    EndIf  
    
    If X_old>X_new And Y_old>Y_new Then
      Azimut = Azimut+180
    ElseIf X_old<X_new And Y_old>Y_new Then
      Azimut = Azimut+90
    ElseIf X_old>X_new And Y_old<Y_new Then
      Azimut = Azimut+270
    EndIf
  Else
    If X_old = X_new And Y_old < Y_new Then
      Azimut = 0
    ElseIf X_old = X_new And Y_old > Y_new Then
      Azimut = 180
    ElseIf X_old < X_new And Y_old = Y_new Then
      Azimut = 90
    ElseIf X_old > X_new And Y_old = Y_new Then
      Azimut = -90
    EndIf
  EndIf
  
  'Azimut = Math.Remainder(Azimut + 540, 360) - 180
  
  MinimumTurn()
  Turn()
    
  Dist = Math.SquareRoot((X_old-X_new)*(X_old-X_new) + (Y_old-Y_new)*(Y_old-Y_new))
  Go_Forward()
  
EndSub

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

' Запуск задачи Gyro
Thread.Run = Gyro

' Перед запуском блока следует передать ему значения
' через соответствующие переменные

For i = 1 To PT-1
  X_old = PTx[i-1]
  Y_old = PTy[i-1]
  X_new = PTx[i]
  Y_new = PTy[i]
  
  GotoXY()
  
  If Finish = "False" Then
    X_old = X
    Y_old = Y
    X_new = PTx[0]
    Y_new = PTy[0]
    Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/back_to_start")
    GotoXY()
    i = 1000
    'Program.End()
  Else
    If i<>PT-1 Then
      Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/poi")
      Speaker.Wait()      
      
    EndIf
  EndIf
    
EndFor

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

Azimut_old = Azimut
Azimut = 0

MinimumTurn()
Turn()

Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/poi_finish")
Speaker.Wait()

LCD.Clear()
LCD.Text(1,0,0,2,Drift10 * (Time/10))

Gyro1 = Sensor.ReadRawValue(3,0)
Program.Delay(5000)
Gyro2 = Sensor.ReadRawValue(3,0)
Drift10 = (Gyro1 - Gyro2) / 5

LCD.Text(1,0,20,2,Drift10  * (Time/10))
Speaker.Note(100,"C4",1000)

Program.Delay(10000)

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


Скачать исходный код к нашего проекта можно в GitHub, а на видео ниже - посмотреть, как работает наш NAVIDOZ3R.



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