суббота, 31 марта 2018 г.

Подражая белому кролику

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


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


Дорожный знак было решено построить на основе платы Arduino и сервопривода, а управлять им - используя произвольный ИК-пульт дистанционного управления.

Код программы для дорожного знака выглядит следующим образом:

#define ServoPin 5 
#include <Servo.h>
#include <IRremote.h> 

Servo servo; 
int receiverPin = 11; 
IRrecv irrecv(receiverPin);  
decode_results results;

void setup() {
  Serial.begin(9600);
  irrecv.enableIRIn();    
}

void loop() {
  if (irrecv.decode(&results)) { 
    Serial.println(results.value, HEX); 
    if(results.value == 0xFF609F){ 
      servo.attach(ServoPin);
      servo.write(180);
      delay(1000);
      servo.detach();
    }
    if(results.value == 0xFFE01F){ 
      servo.attach(ServoPin);
      servo.write(0);
      delay(1000);
      servo.detach();
    }
    irrecv.resume();
  }
}


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


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

Алгоритм работы нашего робота таков:

  • Запускаем независимый процесс по непрерывному чтению данных с камеры в объект в памяти
  • Запускаем независимый процесс по управлению двигателями c использованием ПИД-регулятора
  • Запускаем основной процесс, в котором:
  • Берем очередной кадр из объекта в памяти, и переводим в цветовое пространство HSV
  • Отрезаем от кадра нижнюю четверть кадра и, используя функцию вычисления моментов cv2.moments, вычисляем горизонтальное смещение линии относительно центра
  • Передаем данные о смещении относительно линии процессу ПИД-регулятора
  • Отрезаем нижнюю половину кадра и анализируем количество "синих" точек в нем. Если их количество превышает заданную границу - сообщаем процессу ПИД-регулятора о снижении скорости движения
  • Анализируем количество "красных" точек во всем кадре. Если их количество превышает заданную границу - сообщаем процессу ПИД-регулятора о нулевой скорости движения
Робот видит примерно такую картинку:


Линию в нижней четверти кадра робот видит так:


А вот так он выделяет в кадре красный знак:


Исходный код программы робота:

#!/usr/bin/env python3

import threading
import rpyc
import numpy as np
from imutils.video import WebcamVideoStream
import cv2, time

conn = rpyc.classic.connect('192.168.32.209')
ev3 = conn.modules['ev3dev.ev3']

btn = ev3.Button()

mA = ev3.LargeMotor('outA')
mD = ev3.LargeMotor('outB')

P = 0.4
D = 1.0
K = 0
speed = 300
speed_go = 0
max = 0
x = 0
es = 0
dArea = 0
size = 0

cap = WebcamVideoStream(src=0).start()
for i in range(10): frame = cap.read()
hsv = cv2.cvtColor(frame[:][360:], cv2.COLOR_BGR2HSV)
frame_gray = cv2.inRange(hsv, (0, 0, 0), (255, 170, 80))
cv2.imwrite('/var/www/html/camera_bot_line.png', frame_gray)

def motor_control():
    global x, D, P, frame_gray, speed, es, speed_go
    while True:
        if(speed_go < speed): speed_go += 50
        if(speed_go > speed): speed_go -= 50

        if(speed_go < 150):
            P = 0.15
            D = 0.2
        else:
            P = 0.4
            D = 1.0

        if(speed_go < 5): 
            P = 0
            D = 0
        else:
            P = 0.4
            D = 1.0

        speedA = speed_go - ((x*P) + D*(x-es))
        speedD = speed_go + ((x*P) + D*(x-es))
        if(speedA > 900): speedA = 900
        if(speedA < -900): speedA = -900
        if(speedD > 900): speedD = 900
        if(speedD < -900): speedD = -900
        mA.run_forever(speed_sp=speedA)
        mD.run_forever(speed_sp=speedD)
        es = x

t1 = threading.Thread(target=motor_control)
t1.daemon = True
t1.start()

while True:
    frame = cap.read()
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    frame_gray = cv2.inRange(hsv[:][360:], (0, 0, 0), (255, 170, 80))
    frame_blue = cv2.inRange(hsv[:][240:], (90, 120, 60), (135, 250, 250))
    frame_red = cv2.inRange(hsv, (150, 100, 60), (255, 250, 250))

    if(np.sum(frame_blue) > 30000):
        speed = 100
    elif(np.sum(frame_red) > 80000):
        speed = 0
    else:
        speed = 300

    moments = cv2.moments(frame_gray, 1)
    dM01 = moments['m01']
    dM10 = moments['m10']
    dArea = moments['m00']
    if(dArea != 0):
        x = 320 - int(dM10 / dArea)
        y = 80 - int(dM01 / dArea)

    if(btn.backspace):
        speed = 0
        speed_go = 0
        x = 0
        break

mA.stop(stop_action="brake")
mD.stop(stop_action="brake")


суббота, 10 марта 2018 г.

Следуй за белым кроликом

Машинное зрение использует достаточно ресурсоемкие алгоритмы и, хотя ev3dev на платформе LEGO Mindstorms EV3 и позволяет использовать веб-камеру с библиотеками OpenCV, производительность этой связки оставляет желать лучшего. С целью получения большей вычислительной мощности на борту, но в тоже время не отказываясь от удобных элементов прототипирования, входящих в наборы LEGO, мы решили использовать Raspberry Pi, который будет осуществлять все расчеты, оставляя блоку EV3 задачу низкоуровнего управления двигателями.


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


В проекте мы используем Raspberry Pi второй версии. 4 ядра на борту дают возможность распределить по ним задачи:
1) получение потока данных с камеры
2) постобработка изображения и выделение в кадрах искомого объекта, анализ его размера и положения
3) управление двигателями посредством двух ПИД-регуляторов.


Для связи между Raspberry Pi и EV3 можно использовать как проводное подключение через USB (настроив сетевой LAN-интерфейс), так и беспроводную связь через WiFi/Bluetooth. Мы используем пару WiFi-адаптеров, один в EV3, другой в Raspberry Pi. Питание Raspberry Pi получает от повербанка, который робот также везет на себе. Для "удаленного" управления двигателями мы используем библиотеку RPyC (Remote Python Call).


Алгоритм работы робота следующий:
1) Получаем единичный кадр с камеры
2) Анализируем цвет объекта и размер объекта, расположенного по центру кадра, именно за ним мы и будем следовать в дальнейшем
3) запускаем процесс чтения данных с камеры, получая объект в памяти, содержащий в каждый момент времени актуальную картинку
4) циклично анализируем кадр и пытаемся найти на нем искомый объект. Если объект найден - определяем его горизонтальное смещение и изменение размера относительно эталонного
5) передаем процессу с ПИД-регуляторами данные о горизонтальном смещении и отклонении в размере
6) ПИД регуляторы вырабатывают управляющие воздействия, которое суммируются и результат передается подсистеме управления двигателями.


Исходный код робота, следующего за белым кроликом выглядит так:

#!/usr/bin/env python3

import threading
import rpyc
import numpy as np
from imutils.video import WebcamVideoStream
import cv2, time

conn = rpyc.classic.connect('192.168.32.209')
ev3 = conn.modules['ev3dev.ev3']

btn = ev3.Button()

mA = ev3.LargeMotor('outA')
mD = ev3.LargeMotor('outB')

P = 0.2
speed = 0
max = 0
x = 0
dArea = 0
size = 0

cap = WebcamVideoStream(src=0).start()
for i in range(10): frame = cap.read()

hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
H = hsv[320,240, 0]
S = hsv[320,240, 1]
V = hsv[320,240, 2]
Lo = np.array([H-35, S-50, V-75])
Hi = np.array([H+35, S+50, V+75])

frame_gray = cv2.inRange(hsv, Lo, Hi)
cv2.imwrite('/var/www/html/kube_search.png', frame_gray)

def motor_control():
    global dArea, x, P, size, frame_gray
    while True:
        if(np.sum(frame_gray) > 10000):
            speedA = size*3 - (x*P)
            speedD = size*3 + (x*P)

            if(speedA > 900): speedA = 900
            if(speedA < -900): speedA = -900
            if(speedD > 900): speedD = 900
            if(speedD < -900): speedD = -900

            mA.run_forever(speed_sp=speedA)
            mD.run_forever(speed_sp=speedD)
        else:
            mA.stop(stop_action="brake")
            mD.stop(stop_action="brake")

t1 = threading.Thread(target=motor_control)
t1.daemon = True
t1.start()

while True:
    frame = cap.read()
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    frame_gray = cv2.inRange(hsv, Lo, Hi)
    if(np.sum(frame_gray) > 10000):

        moments = cv2.moments(frame_gray, 1)
        dM01 = moments['m01']
        dM10 = moments['m10']
        dArea = moments['m00']

        if(max==0): max = dArea
        size = 45 - (dArea / max * 100)

        if(dArea != 0):
            x = 320 - int(dM10 / dArea)
            y = 240 - int(dM01 / dArea)

        if(x>-10 and x < 10): x = 0
        if(size>-5 and size < 5): size = 0

    if(btn.backspace):
        size = 0
        x = 0
        break

mA.stop(stop_action="brake")
mD.stop(stop_action="brake")

НейроТир

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


"Пистолет", из которого будет вестись стрельба по мишени, спроектирован на основе LEGO Mindstorrms NXT. Для стрельбы мы будем использовать лазерную указку, включенную на постоянное свечение. Луч лазера будет перекрываться специальной заслонкой, управляемой малым EV3-мотором и ПИД-регулятором.В роли спускового курка выступает датчик кнопка.


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

Программа для пистолета выглядит следующим образом:

#define BT_CONN 1
#define OUTBOX 5

long time=0;
int tri =1;
int triger=1;
int yes=0;
int es=0;
int u=0;
int Pk=3.5;
int PD=7;
int e=0;

void Send2BT()
{
   SendRemoteNumber(BT_CONN,OUTBOX,1);
}
void BTConnect()
{
  CommBTConnectionType args;
  args.Name = "ks2";
  args.ConnectionSlot = BT_CONN;
  args.Action = true;
  if(BluetoothStatus(BT_CONN)!=NO_ERR) SysCommBTConnection(args);
}

task pid()
{
  es=0;
  while(true){
    e = MotorRotationCount(OUT_A)-yes;
    u = e*Pk+PD*(e-es);
    if(u>100){
     u=100;
    }
    if(u<-100){
      u=-100;
    }
    OnRev(OUT_A,u);
    es = e;
  }
}

task main()
{
  ResetRotationCount(OUT_A);
  start pid;
  time=CurrentTick();
  SetSensorTouch(IN_1);
  BTConnect();
  while(true){
    if(Sensor(IN_1)==1){
      yes=60 * triger;
      triger=triger*-1;
      for(int i=1500;i>300;i=i-50)
      {
        PlayTone(i,10);
        Wait(10);

      }
      Send2BT();
    }
    if(CurrentTick()-time>=500){
      time=CurrentTick();
      yes=0;
      Wait(700);
    }
  }
}

Инструкции по сборке в формате LEGO Digital Designer выкладываем по ссылке.



В основе мишени лежит еще один блок NXT, который управляет тремя NXT-моторами. Каждый мотор управляет заслонкой соответствующей цели, в качестве которой используется датчик освещенности. Для того, чтобы увеличить "зону поражения" лазером, сберечь фотодиод датчика при прямых попаданиях, используется рассеивающие фильтры из бумаги, а для устранения фоновой засветки - бленды из покрышек, как в проекте "LEGO UART: бит за битом". Если цель поражена - соответствующая заслонка закрывается, если игрок промахнулся - не пораженная цель вспыхивает красным цветом (используестя подсветка датчиков освещенности).


Нейронная сеть обучается на основе истории попаданий игрока в мишень, учитывается 3 последних выстрела.


Если представить алгоритм обучения нейронной сети в упрощенной форме в терминологии "коробков и камушков", получится следующее:
  • Возьмем 64 пустые коробки и расположим их в виде куба 4x4x4, в каждую такую коробку положим по 1 камешку трех разных цветов (каждый цвет соответствует одной из целей) - это "мозг" нашего самообучающегося робота
  • Возьмем лист бумаги и будем записывать на него историю выстрелов, в какую мишень в каком порядке стрелял игрок. Из этой истории нас будут интересовать три последние выстрела.
  • Предположим игрок уже сделал 3 выстрела и их история зафиксирована на бумаге, например 3,1,2. Первые три цели выбираются случайным образом, нейронная сеть в их выборе не участвует.
  • В "кубе" с коробками выбираем 3 коробку в 1 ряду во втором слое и наугад вытягиваем один из камней, смотрим его цвет и кладем обратно. В зависимости от цвета камня поднимаем соответствующую мишень, предлагая игроку попасть в нее.
  • Если игрок промахнулся мимо выставленной цели, добавляем в коробку камень того цвета, который достали.
  • Если игрок попал в выставленную цель, убираем из коробки вытащенный камень проверив предварительно что в ней есть еще камне такого цвета. Последний камень каждого цвета всегда оставляем в коробке, таким образом число камней в коробке не может быть меньше 3 (по 1 каждого цвета).
  • Фиксируем в истории выстрелов цель, в которую стрелял игрок, например 3, Получаем историю - 3,1,2,3. "Забываем" самый первый выстрел, помним всегда только последние 3 - итого в истории 1,2,3.
  • Переходим на шаг 4, оперируя новой историей выстрелов


#define BT_CONN 0
#define INBOX 5

float l1,l2,l3;
int BTMailbox = 0;
long BTtime = 0;
float random;
long time1, time2, time3;
int x=0, y=0, z=0;
int triger = 1, triger2 = 0;
int target=0;
int history[3]={0,0,0};
int brain[4][4][4][4];
int position1=0, position2=0, position3=0;
int es1=0, es2=0, es3=0;
int u1=0, u2=0, u3=0;
int e1,e2,e3;
float Pk=3;
float PD=6;

bool BTCheck(int conn)
{
  if (BluetoothStatus(conn) == NO_ERR) return true;
  else return false;
}

task pid()
{
  while(true){
    e1 = MotorRotationCount(OUT_A)-position1;
    e2 = MotorRotationCount(OUT_B)-position2;
    e3 = MotorRotationCount(OUT_C)-position3;
    u1 = e1*Pk+PD*(e1-es1);
    u2 = e2*Pk+PD*(e2-es2);
    u3 = e3*Pk+PD*(e3-es3);
    if(u1>100) u1=100;
    if(u2>100) u2=100;
    if(u3>100) u3=100;
    if(u1<-100) u1=-100;
    if(u2<-100) u2=-100;
    if(u3<-100) u3=-100;
    if(abs(e1)>15) OnRev(OUT_A,u1);
    else Off(OUT_A);
    es1 = e1;
    if(abs(e2)>15) OnRev(OUT_B,u2);
    else Off(OUT_B);
    es2 = e2;
    if(abs(e3)>15) OnRev(OUT_C,u3);
    else Off(OUT_C);
    es3 = e3;
  }
}
task main(){
  OnFwd(OUT_ABC,18);
  Wait(3000);
  for(int x=0;x<=3;x++){
    for(int y=0;y<=3;y++){
      for(int z=0;z<=3;z++){
        for(int t=0;t<=3;t++){
          brain[x][y][z][t]=1;
        }
      }
    }
  }
  ResetRotationCount(OUT_A);
  ResetRotationCount(OUT_B);
  ResetRotationCount(OUT_C);
  Wait(3000);
  Off(OUT_ABC);

  start pid;

  SetSensorColorNone(IN_3);
  SetSensorLight(IN_2,0);
  SetSensorLight(IN_1,0);

  position1=-80;
  position2=-80;
  position3=-80;
  Wait(500);

  l1=Sensor(IN_1);
  l2=Sensor(IN_2);
  l3=Sensor(IN_3);

  position1=-30;
  position2=-30;
  position3=-30;
  while(true){
    if(triger>0){
      x=brain[history[0]][history[1]][history[2]][1];
      y=brain[history[0]][history[1]][history[2]][2];
      z=brain[history[0]][history[1]][history[2]][3];
      random = abs(Random((x+y+z)*1000)/1000.0);
      if(random<=x ){
        position1=-80;
        target=1;
      }
      else {
        if(random<=x+y){
          position2=-80;
          target=2;
        }
        else{
          if(random<=x+y+z){
            position3=-80;
            target=3;
          }
        }
      }
    }
    BTMailbox=0;
    while(BTMailbox==0){
      if (BTCheck(BT_CONN)) {
        ReceiveRemoteNumber(INBOX, true, BTMailbox);
      }
    }
    triger=0;
    if(target == 1){
      if(Sensor(IN_1)>=l1+0.01){
        position1= -30;
        Wait(1000);
        triger+=1;
      }
      else{
        triger2=1;
        SetSensorLight(IN_1,1);
        Wait(500);
        SetSensorLight(IN_1,0);
      }
    }
    if(target== 2){
      if(Sensor(IN_2)>=l2+0.01){
        position2= -30;
        Wait(1000);
        triger+=1;
      }
      else{
        SetSensorLight(IN_2,1);
        Wait(500);
        SetSensorLight(IN_2,0);
        triger2=2;
      }
    }
    if(target==3){
      if(Sensor(IN_3)>=l3+0.01){
        position3= -30;
        Wait(1000);
        triger+=1;
      }
      else{
        SetSensorColorRed(IN_3);
        Wait(500);
        SetSensorColorNone(IN_3);
        triger2=3;
      }
    }
    if(triger==0) brain[history[0]][history[1]][history[2]][target]+=1;
    else{
      if(brain[history[0]][history[1]][history[2]][target]>1){
        brain[history[0]][history[1]][history[2]][target]-=1;
      }
    }
    history[0]=history[1];
    history[1]=history[2];
    history[2]=target;
  }
}

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




воскресенье, 28 января 2018 г.

Voic3 Command3R

Юбилейный, сотый, проект от "Карандаша и Самоделкина" просто обязан быть чем-то особенным! В прошлых сериях мы успели поработать в области машинного зрения, и наш робот на базе LEGO Mindstorms EV3 смог различать простейшие образы и даже сыграть с нами в "Камень, ножницы, бумага". На этот раз мы решили освоить работу со звуком и решить одну из задач, связанных с "машинным слухом".

Сделать робота, управляемого голосом, не так уж и сложно - достаточно воспользоваться готовым API от того же Google. В этом случае распознаваемый фрагмент отсылается онлайн-сервису, который в ответ сообщает распознанную текстовую строку. Если роботу нужна подобная автономная функциональность, существуют и готовые оффлайновые решения, вроде PocketSphinx, правда работают они в условиях ограниченных ресурсов на роботе крайне задумчиво. Главной же фишкой нашего проекта станет то, что мы не будем использовать никакие вспомогательные инструменты для распознавания речи, а напишем свой собственный "движок" для голосового управления, пусть и простенький.


Перво-наперво нам понадобится устройство, способное дать возможность роботу физически ощущать звуковую волну для возможности преобразования ее в цифровую форму. В комплекте с наборами LEGO NXT первой версии поставлялся датчик звука (NXT Sound Sensor). C его помощью можно измерять звуковое давление в условных единицах. Такого датчика у нас нет, поэтому мы решили использовать USB-микрофон, подключенный в соответствующий порт блока EV3. Что касается датчика звука NXT, то по информации от его владельцев, скорость опроса этого датчика невелика, поэтому на него рассчитывать все равно не стоит.
В качестве USB-микрофона может выступать почти любая USB-звуковая плата, с подключенным к ней аналоговым микрофоном или USB-веб-камера, как правило имеющая встроенный микрофон. EV3-блок тоже имеет в своем составе звуковую плату, к выходу которой подключен встроенный динамик, а вот встроенного микрофона, увы, нет..Максимальная частота дискретизации этого устройства - 22кГц, поэтому качественно воспроизводить звук EV3-блок не может физически.

При подключении дополнительного USB звукового устройства появляется возможность работать с ними одновременно, например получая данные с микрофона внешней звуковой платы и воспроизводя звук по прежнему встроенным динамиком. Возможна и конфигурация, при которой звук будет воспроизводиться внешней USB звуковой платой, при этом появляется возможность подключить к роботу качественную или "громкую" акустику.
В проекте мы снова используем операционную систему ev3dev и будем программировать робота на языке Python. Для работы с звуковыми устройствами существует целый ряд python-модулей, мы остановили свой выбор на pyalsaaudio, обладающим высоким быстродействием в условиях ограниченных ресурсов. Для ускорения обработки данных и воспользуемся модулем NumPy. Хотя можно хранить данные и в традиционных для Python структурах, вроде списков, однако работа с звуковыми потоками, имеющими зачастую немалый объем, при таком подходе происходит крайне неторопливо.


Общий алгоритм работы нашего робота таков:
1) Стартуем процесс записи и анализируем приходящие со звуковой платы данные небольшими порциями, оценивая громкость. Как только она превысит заданный порог - начинаем запись заданной (избыточной для фразы) длительности, например 2 секунлы
2) Анализируем полученный звуковой отрезок (сэмпл) на предмет выделения на нем слова (фразы).


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


3) Воспроизводим сэмпл для мониторинга корректности его записи и выделения слова.
4) Разбиваем звуковой фрагмент на небольшие интервалы, длительностью около 20 мс. Лучший результат получается если разбивать на интервалы, перекрывающиеся н 50%, в этом случае скорость произношения фразы не будет заметно влиять на результат распознавания.


5) Анализируем каждый отрезок, выделяя в нем частоты с максимальной "громкостью". Частотный анализ фрагмента можно выполнить используя встроенную в NumPy функция быстрого преобразования Фурье.


На рисунке выделено 8 таких частот.
6) Получаем двумерную матрицу, содержащую несколько наиболее выраженным частот в каждом отрезке:


7) Сохраняем матрицу в виде именованного образца. Для каждого слова (голосовой команды) нужно записать несколько таких образков, с разной интонацией, можно разными голосами, если это необходимо.
8) Чтобы распознать слово-команду необходимо выполнить наги 1-5 и сравнить полученную матрицу с сохраненными образцами. Здесь есть несколько вариантов:
  • Найти самый похожий образец
  • Найти группу образцов, к которой анализируемый ближе всего
  • Скомбинировать поиск по близости к образцу и по близости к группе образцов. Если, например, анализируемый образец ближе всего к группе с образцами "Влево" и, одновременно, ближе в образцу "Вправо", у робота появляется возможность ответить "Не понял команды, повторите!".
  • Использовать нейронную сеть, обученную на записанных образцах.
В проекте мы используем третий вариант, а четвертый в данный момент изучаем.

суббота, 27 января 2018 г.

MindSnake3D

Пару недель назад в нашей группе VK был запущен опрос, для чего предназначен робот на фото?


Подсказки были таковы:
1) проект носит развлекательный характер
2) робот использует трехмерные массивы
3) разработка проекта ведется на языке С, однако сам проект тесно связан с другим языком программирования, который нам очень нравится
4) Шелдону Куперу он наверняка пришелся бы по душе.


Пришла пора приоткрыть завесу тайны. На каникулах мы тоже любим отдыхать, поэтому наш очередной проект, о котором сегодня пойдет речь, действительно носит развлекательный характер. Наверное многие из вас играли в знаменитую игру Snake ("Змейка"), она портирована на множество различных платформ, а наибольшую популярность получила будучи предустановленной на телефонах Nokia (подсказка №3: Питон - змея).


Змейка ползает по полю, поедая пищу, удлинняясь от каждой съеденной порции. Если змейка врезается в ограждение поля или в свой хвост - она погибает и игра начинается сначала. Цель игры - вырастить как можно более длинную змейку.
Мы давно хотели сделать эту игру на платформе LEGO Mindstorms, но интересная реализация никак не вырисовывалась. Создавать очередной клон на экране NXT или EV3 блока нам не хотелось, а реализовать змейку в механике похоже не по зубам даже гуру LEGO-роботостроения. 
Наконец, когда пришла пора изучить трехмерные массивы - все сложилось и идея обрела законченную форму. Далеким прототипом стал наш наш проект 2015 года - EV3 Муха. В этой игре в уме (или "мыслилке", как мы называем такого типа развлечения) мы должны были двигать воображаемую муху по плоскому полю. 
В проекте MinSnake мы добавим пищи для ума в виде еще одного измерения. Наша змейка будет ползать в 3D-кубе, который игрок должен представить в уме, управление головой змеи мы реализовали в конструкции. Все игровые события робот озвучивает голосом, экран блока в геймплее не используется. Змейка как и в классическом варианте не должна выползти за пределы игрового поля (куба) и не должна врезаться сама в себя (в свой хвост). Заморочено, правла? Шелдону Куперу точно бы понравилось!



Исходные данные таковы:
- куб имеет размер 3x3 пиксела
- змейка на старте имеет длину = 1, стартует из центра (ячейка 2,2,2)
- стартовое направление в начале игры - направо
- в зависимости от выбранного уровня сложности на ход дается в среднем 5 секунд, за это время игрок должен выбрать одно из 6 направлений движения (вверх-вниз-влево-вправо-к себе-от себя), в соответствии с которым змейка передвинется по истечении времени хода. На каждый следующий ход дается на несколько миллисекунд меньше чем на предыдущий
- еда появляется в кубе случайным образом, на свободном от змейки месте. Робот называет ячейку где именно. Чтобы не путаться в координатах озвучивается слой сверху вниз (1-2-3) и номер ячейки 1..9 по телефонному принципу (см. рис)


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


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



Исходный код программы на языке NXC:
struct coord{
  int x;
  int y;
  int z;
};

coord head;
coord eda;
coord max;

int snake[3][3][3];

void food(){
  while(true){
    eda.x=Random(2999)/1000.0;
    eda.y=Random(2999)/1000.0;
    eda.z=Random(2999)/1000.0;
    if(snake[eda.x][eda.y][eda.z]==0){
      break;
    }
  }
}

int otkl(int a, int b)
{
  return ((a+36000)%360 + 540 - b)%360 - 180;
}

int direct(){
  if(abs(otkl(MotorRotationCount(OUT_B),0))<=45){
    return 4;
  }
  if(abs(otkl(MotorRotationCount(OUT_B),180))<=45){
    return 5;
  }

  if(abs(otkl(MotorRotationCount(OUT_A),0))<=45 && abs(otkl(MotorRotationCount(OUT_B),90))<=45){
    return 3;
  }

  if(abs(otkl(MotorRotationCount(OUT_A),0))<=45 && abs(otkl(MotorRotationCount(OUT_B),270))<=45){
    return 2;
  }

  if(abs(otkl(MotorRotationCount(OUT_A),90))<=45 && abs(otkl(MotorRotationCount(OUT_B),90))<=45){
    return 1;
  }

  if(abs(otkl(MotorRotationCount(OUT_A),90))<=45 && abs(otkl(MotorRotationCount(OUT_B),270))<=45){
    return 0;
  }

  if(abs(otkl(MotorRotationCount(OUT_A),180))<=45 && abs(otkl(MotorRotationCount(OUT_B),90))<=45){
    return 3;
  }

  if(abs(otkl(MotorRotationCount(OUT_A),180))<=45 && abs(otkl(MotorRotationCount(OUT_B),270))<=45){
    return 2;
  }

  if(abs(otkl(MotorRotationCount(OUT_A),270))<=45 && abs(otkl(MotorRotationCount(OUT_B),90))<=45){
    return 1;
  }

  if(abs(otkl(MotorRotationCount(OUT_A),270))<=45 && abs(otkl(MotorRotationCount(OUT_B),270))<=45){
    return 0;
  }

  return -1;
}
int time_hod=5000;
int direction=4;//0-туда1-обратно2-вверх3-вниз4-вправо5-влево
int eda_farther=0;
int maxa=0;
int direction_old=4;
long ti;
int len=1;
int level=0;
int len_up;
task main()
{
  ClearScreen();
  //TextOut(0,16,"Easy");
  //TextOut(0,8,"Normal");
  //TextOut(0,0,"Hard");
  Wait(1000);
  while(ButtonPressed(BTNCENTER,0)
  && ButtonPressed(BTNLEFT,0)
  && ButtonPressed(BTNRIGHT,0)){

  }
  if(ButtonPressed(BTNCENTER,1)){
    level=2;
    len_up=9;
    time_hod=5000;
    ClearScreen();
  //  TextOut(0,0,"Normal");
  }
  if(ButtonPressed(BTNLEFT,1)){
    level=1;
    len_up=3;
    time_hod=5000;
    ClearScreen();
  //  TextOut(0,0,"Easy");
  }
  if(ButtonPressed(BTNRIGHT,1)){
    level=3;
    len_up=9;
    time_hod=3000;
    ClearScreen();
  //  TextOut(0,0,"Hard");
  }
  Wait(3000);

  eda.x=10;
  eda.y=10;
  eda.z=10;
  head.x=1;
  head.y=1;
  head.z=1;
  PlayFile("start.rso");
  PlayFile("center.rso");
  ResetRotationCount(OUT_AB);
  direction=direct();
  for(int i=0;i<=2;i++){
    for(int z=0;z<=2;z++){
      for(int q=0;q<=2;q++){
        snake[i][z][q]=0;
      }
    }
  }
  snake[1][1][1]=1;
  long time=CurrentTick();
  long time_eda=CurrentTick();
  while(true){
    if(len==len_up && level==3){
      break;
      PlayFile("levelup.rso");
    }
    if(len==len_up && level==2){
      level=3;
      len_up=9;
      time_hod=3000;
      PlayFile("levelup.rso");
      Wait(1000);
    }
    if(len==len_up && level==1){
      level=2;
      len_up=6;
      PlayFile("levelup.rso");
      Wait(1000);
    }

    direction_old=direction;
    direction=direct();

    if(direction!=direction_old){
      switch(direction)
      {
      case 0:
        PlayFile("farther.rso");
        Wait(1000);
        break;
       case 1:
         PlayFile("closer.rso");
         Wait(1000);
         break;
       case 2:
         PlayFile("up.rso");
         Wait(1000);
         break;
       case 3:
         PlayFile("down.rso");
         Wait(1000);
         break;
       case 4:
         PlayFile("right.rso");
         Wait(1000);
         break;
       case 5:
         PlayFile("left.rso");
         Wait(1000);
         break;
       default:
         break;
       }
    }
    if(CurrentTick()-time_eda>=20000){
      time_eda=CurrentTick();
      food();
      PlayFile("food.rso");
      Wait(1000);
      if(eda.y==0){
        PlayFile("3.rso");
        Wait(1000);
      }
      if(eda.y==1){
        PlayFile("2.rso");
        Wait(1000);
      }
      if(eda.y==2){
        PlayFile("1.rso");
        Wait(1000);
      }
      if(eda.z==2){
        if(eda.x==0){
          PlayFile("1.rso");
          Wait(1000);
        }
        if(eda.x==1){
          PlayFile("2.rso");
          Wait(1000);
        }
        if(eda.x==2){
          PlayFile("3.rso");
          Wait(1000);
        }
      }
      if(eda.z==1){
        if(eda.x==0){
          PlayFile("4.rso");
          Wait(1000);
        }
        if(eda.x==1){
          PlayFile("5.rso");
          Wait(1000);
        }
        if(eda.x==2)
        {
          PlayFile("6.rso");
          Wait(1000);
        }
      }
      if(eda.z==0){
        if(eda.x==0){
          PlayFile("7.rso");
          Wait(1000);
        }
        if(eda.x==1){
          PlayFile("8.rso");
          Wait(1000);
        }
        if(eda.x==2){
          PlayFile("9.rso");
          Wait(1000);
        }
      }

    }
    if(CurrentTick() - ti>=1000){
      ti=CurrentTick();
 //     ClearScreen();
     // NumOut(10,0,eda.z);
    // / NumOut(10,8,eda.y);
    //  NumOut(10,16,eda.x);
    //  NumOut(0,0,head.z);
   ///   NumOut(0,8,head.y);
    //  NumOut(0,16,head.x);
    //  NumOut(0,24,direction);
     // NumOut(5,0,len);
    }
    if(CurrentTick()-time>=time_hod){
      PlayTone(440,100);
      time_hod=time_hod-10;
     
      time=CurrentTick()
      if(head.x==2 && direction==4
      || head.x==0 && direction==5
      || head.y==2 && direction==2
      || head.y==0 && direction==3
      || head.z==2 && direction==0
      || head.z==0 && direction==1){
        break;
      }
   
      eda_farther=0;
   
      if(direction==0 && head.x==eda.x && head.y==eda.y && head.z+1==eda.z){
        eda_farther=1;
      }
      if(direction==1 && head.x==eda.x && head.y==eda.y && head.z-1==eda.z){
        eda_farther=1;
      }
      if(direction==2 && head.x==eda.x && head.y+1==eda.y && head.z==eda.z){
        eda_farther=1;
      }
      if(direction==3 && head.x==eda.x && head.y-1==eda.y && head.z==eda.z){
        eda_farther=1;
      }
      if(direction==4 && head.x+1==eda.x && head.y==eda.y && head.z==eda.z){
        eda_farther=1;
      }
      if(direction==5 && head.x-1==eda.x && head.y==eda.y && head.z==eda.z){
        eda_farther=1;
      }
      for(int i=0;i<=2;i++){
        for(int z=0;z<=2;z++){
          for(int q=0;q<=2;q++){
            if(snake[i][z][q]!=0){
              snake[i][z][q]+=1;
            }
          }
        }
      }
      max=0;
      for(int i=0;i<=2;i++){
        for(int z=0;z<=2;z++){
          for(int q=0;q<=2;q++){
            if(snake[i][z][q]>maxa){
              maxa=snake[i][z][q];
              max.x=i;
              max.y=z;
              max.z=q;
            }
          }
        }
      }

      if(eda_farther==0){
        snake[max.x][max.y][max.z]=0;
      }
      else{
        len++;
        time_eda=CurrentTick();
        eda.x=10;
        eda.y=10;
        eda.z=10;
        PlayFile("len.rso");
        Wait(1000);
      }
      maxa=0;
      max.x=0;
      max.y=0;
      max.z=0;
      if(direction==0){
        if(snake[head.x][head.y][head.z+1]==0){
          snake[head.x][head.y][head.z+1]=1;
          head.z++;
        }
        else{
          break;
        }
      }
      if(direction==1){
        if(snake[head.x][head.y][head.z-1]==0){
          snake[head.x][head.y][head.z-1]=1;
          head.z--;
        }
        else{
          break;
        }
      }
      if(direction==2){
        if(snake[head.x][head.y+1][head.z]==0){
          snake[head.x][head.y+1][head.z]=1;
          head.y++;
        }
        else{
          break;
        }
      }
      if(direction==3){
        if(snake[head.x][head.y-1][head.z]==0){
          snake[head.x][head.y-1][head.z]=1;
          head.y--;
        }
        else{
          break;
        }
      }
      if(direction==4){
        if(snake[head.x+1][head.y][head.z]==0){
          snake[head.x+1][head.y][head.z]=1;
          head.x++;
        }
        else{
          break;
        }
      }
      if(direction==5){
        if(snake[head.x-1][head.y][head.z]==0){
          snake[head.x-1][head.y][head.z]=1;
          head.x--;
        }
        else{
          break;
        }
      }
      direction=direct();
    }
  }
  PlayFile("end.rso");
  Wait(1000);
}


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