LEGO Laser Pong

Судя по нашим последним работам может показаться, что нам так понравилась связка EV3 и EV3 Basic, что все проекты мы готовы делать только с их применением. EV3 Basic - действительно классная штука, однако мы отдаем себе отчет в том, что у него есть ряд ограничений и используем его по большей части как промежуточный, переходный инструмент, на пути от графического программирования к полноценному текстовому. В нашем сегодняшнем проекте LEGO Laser Pong мы вернемся к истокам и будем использовать старый добрый Mindstorms NXT. В качестве языка программирования в данном проекте мы выбрали NXС - замечательный инструмент разработки для платформ NXT и RСX. Подробнее о нем вы можете почитать в статье Антона Ботова "Программирование LEGO NXT роботов на языке NXC", являющейся переводом статьи известного LEGO-гуру Daniele Benedettelli.
NXC на наш взгляд - одна из тех крутых вещей, которая до сих пор дает платформе NXT огромное преимущество над EV3. Конечно, можно возразить, что RobotC расставляет все точки над i, но давайте вспомним его стоимость и сравним с свободно распространяемым NXC. Когда покупаешь ПО за свои деньги, а не за бюджетные или деньги клуба, а компьютеров, на которых программируют роботов несколько - это иногда имеет значение. К тому же NXC умеет работать с базовой прошивкой NXT и перепрошивать блок не приходится.

Pong является одной из самых ранних аркадных видеоигр, это теннисная спортивная игра с использованием простой двухмерной графики. Цель игры состоит в том, чтобы победить противника в настольный теннис, зарабатывая очки. Игра была создана фирмой Atari, которая выпустила её в 1972 году. Pong быстро стал популярным и стал первой коммерчески успешной видеоигрой. Вскоре после того, как Pong стал открытым, некоторые компании начали производить игры, которые копировали геймплей Pong'а.

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

Мы неоднократно порывались сделать понг на экране блока сначала NXT, а затем и EV3, но каждый раз приходили идеи более интересных проектов, да и мы никак не могли придумать как можно вынести понг из этой "маленькой коробочки с экраном" в реальный мир.

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

Давайте посмотрим как работает наш робот, а затем подробно поговорим о том, как он устроен:

У привода лазерной указки две степени свободы, это позволяет роботу "светить" в любую точку на поверхности игрового поля. Один мотор отклоняет лазерную указку относительно условной оси Х, другой - относительно условной оси Y. Хотя робот и построен в основном с использование компонентов платформы NXT, мы использовали малый мотор EV3 в качестве подвижного мотора, в связи с его небольшим весом.

Одной "ракеткой" управляет человек, используя в качестве нее свою руку, сжатую в кулак. Расположение руки считывается ультразвуковым датчиком.

Второй ракеткой управляет робот. привод этой ракетки устроен следующим образом:

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

В начале игры "мяч" располагается по центру поля, для этого задаются его стартовые координаты x и y в мм относительно начала координат - точки, в которую смотрит лазер в момент запуска программы. После этого вычисляется случайное направление движения naprav в пределах некоторого сектора -60..60 градусов, в который "полетит мяч" в начале игры.

naprav = Random(120)-60;

Направление переводится в диапазон 0..360 градусов формулой

naprav = (naprav + 3600)%360;

Мяч начинает движение в памяти со скоростью speed (мм/сек), каждые 10 мс рассчитываются координаты его нового местоположения

y = y + speed/100 * sin(naprav*PI/180);
x = x + speed/100 * cos(naprav*PI/180);
Wait(MS_10);

naprav*PI/180 - переводит из градусов в радианы.
speed/100 - в связи с расчетом нового положения каждые 1/100 сек

В параллельном процессе рассчитываем углы отклонения лазера в зависимости от текущих x и y. В нем работают пара ПД-регуляторов, которые поворачивают указку под рассчитанными углами: В конструкции используется редуктор 56:1, учитываем это при расчетах углов поворота моторов.

ask MotorsControl()
{
while(true)
  {
  x_ugol = atan(x/z)*180.0/PI;
  
  gip = sqrt(x*x+z*z);
  y_ugol = atan(y/gip)*180.0/PI;

  E_x = x_ugol - MotorRotationCount(OUT_B)/56.0;
  u_x = Pk_x*E_x + Dk_x*(E_x - E_old_x);
  E_old_x = E_x;

  E_y = y_ugol - (-1*MotorRotationCount(OUT_C))/56.0;
  u_y = Pk_y*E_y + Dk_y*(E_y - E_old_y);
  E_old_y = E_y;

  Motor(u_x,-1*u_y);
    
  Wait(5);
  }
} 

Функция Motor(int speed_B,int speed_C) реализует ограничение мощности на моторах в пределах -100..100

void Motor(int speed_B,int speed_C)
  {
  if(speed_B > 100) speed_B = 100;
  if(speed_B < -100) speed_B = -100;

  OnFwd(OUT_B, speed_B);

  if(speed_C > 100) speed_C = 100;
  if(speed_C < -100) speed_C = -100;

  OnFwd(OUT_C, speed_C);
  }

В случаях столкновения мяча с верхней или нижней границей поля мяч отскакивает, угол отражения равен углу падения

if (y<=0 || y>=Y_Max)
  {
  pic = 1;
  naprav = -1*naprav;
  naprav = (naprav + 3600)%360;
  }

Если достигнуты открытые границы поля, анализируется столкновение с ракетками:

    if (x<=-X_Max/2)
      {
      motor[0] = abs(MotorRotationCount(OUT_A)/2.44);
      
      if(y>motor[0]-10 && y<motor[0]+80)
        {
        pic = 2;

        if (abs(((motor[4] - motor[0]) / 4 * 10 + speed * sin(naprav*PI/180)) / speed) < 1) 
          {
          naprav = (asin (((motor[4] - motor[0]) / 4 * 10 + speed * sin(naprav*PI/180)) / speed)) *180 / PI;
          naprav = (naprav + 3600)%360;
          }
        else
          {
          naprav = -1*naprav+180;
          naprav = (naprav + 3600)%360;
          }
        }
      else
        {
        pic = 3;
        gol_i++;

        TextOut(0,LCD_LINE1,"PLAYER1 = ");
        TextOut(0,LCD_LINE3,"PLAYER2 = ");
        NumOut(80,LCD_LINE1,gol_i);
        NumOut(80,LCD_LINE3,gol_v);
        Wait(SEC_3);

        for (int i = -1*X_Max/2;i < 0;i++)
          {
          x = i;
          Wait(MS_10);
          }
        y = Y_Start;
        Wait(SEC_2);

        naprav = Random(120)-60;
        naprav = (naprav + 3600)%360;
        }
      }

    if (x>=X_Max/2)
      {
      US[0] = SensorUS(S4)*10-70;
      
      if(y>US[0]-50 && y<US[0]+70)
        {
        pic = 2;
      
        if (abs(((US[0] - US[4]) / 4 * 10 + speed * sin(naprav*PI/180)) / speed) < 1) 
          {
          naprav = (asin (((US[0] - US[4]) / 4 * 10 + speed * sin(naprav*PI/180)) / speed)) *180 / PI;
          }
        naprav = (naprav + 3600)%360;

        naprav = -1*naprav+180;
        naprav = (naprav + 3600)%360;
        }
      else
        {
        pic = 3;
        gol_v++;
      
        TextOut(0,LCD_LINE1,"PLAYER1 = ");
        TextOut(0,LCD_LINE3,"PLAYER2 = ");
        NumOut(80,LCD_LINE1,gol_i);
        NumOut(80,LCD_LINE3,gol_v);
      
        Wait(SEC_3);

        for (int i = abs(X_Max/2);i > 0;i--)
          {
          x = i;
          Wait(MS_10);
          }
        y = Y_Start;
        Wait(SEC_2);

        naprav = Random(120)+120;
        naprav = (naprav + 3600)%360;
        }
      }

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

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

новое_направление = arcsin ((скорость_ракетки + вертикальная скорость_мяча) / скорость_мяча)

Формула применима в случае, если модуль суммы скоростей ракетки и мяча не превышают скорости мяча.

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

task Protivnik()
{
  long Time_p = CurrentTick();
  int granica = Random(200)-200;
  while(true)
  {
    E_A =y*2.3 - (-1*MotorRotationCount(OUT_A));
    u_A = Pk_R*E_A + Dk_R*(E_A - E_old_A);
    E_old_A = E_A;

    if(u_A > 100) u_A = 100;
    if(u_A < -100) u_A = -100;

    if (Time_p - CurrentTick() > 2000)
      {
      granica = Random(200)-200;
       
      float tmp = Random(50)/100+0.5;
       
      Pk_R = Pk_A*tmp;
      Dk_R = Dk_A*tmp;

      Time_p = CurrentTick();
      }

    if(x<=granica)
      {
      OnRev(OUT_A, u_A);
      }
    else
      {
      Off(OUT_A);
      }
    
    Wait(10);
  }
}

Инструкцию по сборке робота, которую мы традиционно подготовили в формате LEGO Digital Designer, и программу для его работы можно скачать по ссылке.