суббота, 15 сентября 2018 г.

NXT for Speed

Похоже LEGO Mindstorms NXT уже всеми забыт и в мире пластиковой робототехники безраздельно правит EV3. Однако мы не спешим списывать его со счетов и в нашем сегодняшнем проекте он еще даст жару!


NXT for Speed - гоночный аркадный автомат с использованием руля и педалей. Основная цель проекта - изучение основных принципов работы с динамической векторной графикой, закрепление навыков программирования многопоточных приложений на С.


В нашем проекте мы реализовали:
  • динамический расчет конфигурации трассы
  • физическую модель, учитывающую силу нажатия на педаль газа, сопротивление покрышек, сопротивление воздуха. Автомобиль способен двигаться накатом, тормозить с учетом силы нажатия на педаль тормоза.
  • вид от третьего лица с изменением угла зрения на автомобиль
  • "плавающий" горизонт с задним фоном (горы)
  • дорожные знаки на обочинах дороги 
  • управление с использованием руля (контроль угла поворота) и педалей (газ-тормоз)
  • реализован автовозврат педалей - они жесткие и возвращаются в исходную позицию, если их отпустить
  • обратная связь от препятствий - при съезде на обочины руль трясет
  • обратная связь от скорости движения - чем выше скорость тем сложнее резко повернуть руль, на стоящей машине руль поворачивается легко
  • цифровой спидометр в углу экрана
  • подсчет очков

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


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

Код:
int ohki=0;
int trig=1;
int r=23;
int ry=0;
int rx=0;
int mx=0;
int my;
float rz=2;
int l=0;
int c=0;
long t;
float m=500;
int f=4;
float x1=25,x2=75,y1=0,y2=0;
int rt=0;
int o=-10000000000,p=100000000;
int u=0;
float k=0.90;
int tmp =0;
float turn[48];
int ch=0;
float a=50;
int b=0;
int e=0;
int Sign(int x, int y,int v,int c)
{
  if(c==0){
    LineOut(x,y,x,y+v/2,DRAW_OPT_NORMAL);
    RectOut(x-v/4,y+v/2,v/2,v/2,DRAW_OPT_NORMAL);
  }
  else{
    LineOut(x,y,x,y+v/2,DRAW_OPT_CLEAR);
    RectOut(x-v/4,y+v/2,v/2,v/2,DRAW_OPT_CLEAR);
  }
}

int lines(int x, int y,int color)
{
  mx=x;

  if(color==0){
    RectOut(x-6,y+4,11,3,DRAW_OPT_NORMAL);
    RectOut(x-5,y+2,2,2,DRAW_OPT_NORMAL);
    RectOut(x+2,y+2,2,2,DRAW_OPT_NORMAL);
    LineOut(x+5,y+7,x+3,y+10,DRAW_OPT_NORMAL);
    LineOut(x-6,y+7,x-4,y+10,DRAW_OPT_NORMAL);
    LineOut(x-4,y+10,x+3,y+10,DRAW_OPT_NORMAL);
    if(CurrentTick()-t>500){
      TextOut(0,0,"    ");

      e=(500-m)/5;
      NumOut(0,0,e);
      t=CurrentTick();
    }
    if(MotorRotationCount(OUT_A)  > 15){
      LineOut(x+5,y+6,x+9,y+9,DRAW_OPT_NORMAL);
      LineOut(x+3,y+10,x+7,y+12,DRAW_OPT_NORMAL);
      LineOut(x-4,y+10,x-2,y+12,DRAW_OPT_NORMAL);
      LineOut(x,y+12,x+7,y+12,DRAW_OPT_NORMAL);
      LineOut(x+7,y+12,x+10,y+5,DRAW_OPT_NORMAL);
      LineOut(x+6,y+3,x+10,y+5,DRAW_OPT_NORMAL);
      LineOut(x+10,y+4,x+10,y+3,DRAW_OPT_NORMAL);
      LineOut(x+9,y+3,x+10,y+3,DRAW_OPT_NORMAL);

      if(x+10>=p+10){
        TextOut(35,20,"Over");
        rt=1;
      }
    }
    else if(MotorRotationCount(OUT_A)  <-15){

      LineOut(x+3,y+10,x+1,y+12,DRAW_OPT_NORMAL);
      LineOut(x-4,y+10,x-7,y+12,DRAW_OPT_NORMAL);
      LineOut(x+1,y+12,x-8,y+12,DRAW_OPT_NORMAL);
      LineOut(x-6,y+7,x-9,y+10,DRAW_OPT_NORMAL);
      LineOut(x-8,y+11,x-11,y+7,DRAW_OPT_NORMAL);
      LineOut(x-11,y+7,x-7,y+4,DRAW_OPT_NORMAL);
      LineOut(x-11,y+7,x-11,y+4,DRAW_OPT_NORMAL);

      LineOut(x-11,y+4,x-9,y+4,DRAW_OPT_NORMAL);
      if(x-11<=o-10){
        TextOut(35,20,"Over");
        rt=1;
      }
    }
    else{
      LineOut(x+3,y+10,x+1,y+12,DRAW_OPT_NORMAL);
      LineOut(x-4,y+10,x-2,y+12,DRAW_OPT_NORMAL);
      LineOut(x-2,y+12,x+1,y+12,DRAW_OPT_NORMAL);
      LineOut(x-5,y+11,x-5,y+11,DRAW_OPT_NORMAL);
      LineOut(x+4,y+11,x+4,y+11,DRAW_OPT_NORMAL);
      //if(x+10<=p || x-6>=o){
      //  TextOut(0,0,"Over");
      //  rt=1;
      //}
    }
    Wait(10);
  }
  else{
    RectOut(x-6,y+4,11,3,DRAW_OPT_CLEAR);
    RectOut(x-5,y+2,2,2,DRAW_OPT_CLEAR);
    RectOut(x+2,y+2,2,2,DRAW_OPT_CLEAR);
    LineOut(x+5,y+7,x+3,y+10,DRAW_OPT_CLEAR);
    LineOut(x-6,y+7,x-4,y+10,DRAW_OPT_CLEAR);
    LineOut(x-4,y+10,x+3,y+10,DRAW_OPT_CLEAR);

    LineOut(x+5,y+6,x+9,y+9,DRAW_OPT_CLEAR);
    LineOut(x+3,y+10,x+7,y+12,DRAW_OPT_CLEAR);
    LineOut(x-4,y+10,x,y+12,DRAW_OPT_CLEAR);
    LineOut(x,y+12,x+7,y+12,DRAW_OPT_CLEAR);
    LineOut(x+7,y+12,x+10,y+5,DRAW_OPT_CLEAR);
    LineOut(x+6,y+3,x+10,y+5,DRAW_OPT_CLEAR);
    LineOut(x+10,y+4,x+10,y+3,DRAW_OPT_CLEAR);
    LineOut(x+9,y+3,x+10,y+3,DRAW_OPT_CLEAR);
    LineOut(x+3,y+10,x+1,y+12,DRAW_OPT_CLEAR);
    LineOut(x-4,y+10,x-7,y+12,DRAW_OPT_CLEAR);
    LineOut(x+1,y+12,x-8,y+12,DRAW_OPT_CLEAR);
    LineOut(x-6,y+7,x-9,y+10,DRAW_OPT_CLEAR);
    LineOut(x-8,y+11,x-11,y+7,DRAW_OPT_CLEAR);
    LineOut(x-11,y+7,x-7,y+4,DRAW_OPT_CLEAR);
    LineOut(x-11,y+7,x-11,y+4,DRAW_OPT_CLEAR);
    LineOut(x-11,y+4,x-9,y+4,DRAW_OPT_CLEAR);
  }
  return 0;
}

task mot()
{
  Wait(3000);
  float P=0.7;
  //P=0.3*e;
  float Kp=e/100;
  float P2=0.7;
  float P3=0.7;
  float ERR1=0;
  float ERR2=0;
  float ERR3=0;
  float u1=0;
  float u2=0;
  float u3=0;
  while(1){
    if(rt==1){
      break;
    }

    float Kp=e/100;
    ERR1=0-MotorRotationCount(OUT_A);
    u1=P*ERR1*Kp;
    ERR2=0-MotorRotationCount(OUT_B);
    u2=P2*ERR2;
    ERR3=0-MotorRotationCount(OUT_C);
    u3=P3*ERR3;
    if(mx-11<=o+3){
      u1=Random(70)-35;
    }
    if(mx+10>=p-3){
      u1=Random(70)-35;
    }

    if(u1>100)u1=100;
    if(u1<-100)u1=-100;
    if(u1<0){
      OnRev(OUT_A,-1*u1);
    }
    else{
      OnFwd(OUT_A,u1);
    }
    if(u2>100)u2=100;
    if(u2<-100)u2=-100;
    if(u2<0){
      OnRev(OUT_B,-1*u2);
    }
    else{
      OnFwd(OUT_B,u2);
    }
    if(u3>100)u3=100;
    if(u3<-100)u3=-100;
    if(u3<0){
      OnRev(OUT_C,-1*u3);
    }
    else{
      OnFwd(OUT_C,u2);
    }
    Wait(100);
  }
}

task road()
{
  for(int i =0;i<47;i++){
    turn[i]=0;
  }
  while(true){

    ch++;
    f--;
    if(f==0){
      f=4;
    }

    l+=1;

    for(int i =0;i<23;i+=1){

      LineOut(25+turn[i]+i*0.9,i*2,25+turn[i+1]+(i+1)*0.9,i*2+3,DRAW_OPT_NORMAL);
      LineOut(75+turn[i]-i*0.9,i*2,75+turn[i+1]-(i+1)*0.9,i*2+3,DRAW_OPT_NORMAL);
      if(i==r){
        if(trig==1){
          rx=25+turn[i]+i*0.9-5;
          ry=i*2;
        }
        else{
          rx=75+turn[i]-i*0.9+5;
          ry=i*2;
        }
      }
      if(i%4==f  && i>5){
        LineOut(50+turn[i],i*2,50+turn[i+1],i*2+3,DRAW_OPT_NORMAL );
      }
    }
    Sign(rx,ry,rz,0);
    m+=5;
    m+=m*0.2;
    if(m>500){
      m=500;
    }
    if(m<0){
      m=0;
    }
    if(abs(MotorRotationCount(OUT_B))> 5){
      if(m<=500){
        m-=1.7*abs(0-abs(MotorRotationCount(OUT_B))*2);

      }
      if(m>500){
        m=500;
      }
      if(m<0){
        m=0;
      }
    }
    if(abs(MotorRotationCount(OUT_C))> 5){
      if(m>=0){
        m+=1.7*abs(0-abs(MotorRotationCount(OUT_C)));

      }
      if(m>500){
        m=500;
      }
      if(m<0){
        m=0;
      }
    }


    Wait(m);

    for(int i =0;i<23;i+=1){
      LineOut(25+turn[i]+i*0.9,i*2,25+turn[i+1]+(i+1)*0.9,i*2+3,DRAW_OPT_CLEAR);
      LineOut(75+turn[i]-i*0.9,i*2,75+turn[i+1]-(i+1)*0.9,i*2+3,DRAW_OPT_CLEAR);

      if(i%4==f && i>5)
      {
        LineOut(50+turn[i],i*2,50+turn[i+1],i*2+3,DRAW_OPT_CLEAR);
      }
      if(i==3){
        o=25+turn[i];
        p=75+turn[i];
      }
    }
    Sign(rx,ry,rz,1);
    rz+=0.5;
    r--;

    if(rz>=13){
      rz=2;
      r=23;
      trig=trig*-1;
    }
    for(int i =0;i<47;i++){
      turn[i]=turn[i+1];
    }
    if(ch==23){
      ohki+=1;
      ch=0;
      u = Random(100);
      turn[47]=u-50;
      for(int i =23;i<47;i++){
        turn[i]=turn[i-1]*k+turn[47]*(1-k);
      }
    }

    if(rt==1){
      break;
    }
    if(l==1){
      LineOut(70,0,70,35,DRAW_OPT_NORMAL);
      LineOut(70,35,30,35,DRAW_OPT_NORMAL);
      LineOut(70,17,30,17,DRAW_OPT_NORMAL);
      LineOut(70,0,30,0,DRAW_OPT_NORMAL);
      Wait(1000);
      LineOut(70,0,70,35,DRAW_OPT_CLEAR);
      LineOut(70,35,30,35,DRAW_OPT_CLEAR);
      LineOut(70,17,30,17,DRAW_OPT_CLEAR);
      LineOut(70,0,30,0,DRAW_OPT_CLEAR);
      LineOut(30,0,70,35,DRAW_OPT_NORMAL);
      LineOut(30,35,70,35,DRAW_OPT_NORMAL);
      LineOut(70,0,30,0,DRAW_OPT_NORMAL);

      Wait(1000);
      LineOut(30,0,70,35,DRAW_OPT_CLEAR);
      LineOut(30,35,70,35,DRAW_OPT_CLEAR);
      LineOut(70,0,30,0,DRAW_OPT_CLEAR);
      LineOut(30,17,70,35,DRAW_OPT_NORMAL);
      LineOut(70,0,70,35,DRAW_OPT_NORMAL);

      Wait(1000);
      LineOut(30,17,70,35,DRAW_OPT_CLEAR);
      LineOut(70,0,70,35,DRAW_OPT_CLEAR);
    }
  }
}

task main()
{
  t=CurrentTick();
  SetSensorTouch(IN_4);
  start road;
  start mot;

  ClearScreen();
  LineOut(0,48,100,48,DRAW_OPT_NORMAL);

  lines(a,b,0);

  while(true){
    NumOut(80,0,ohki);
    c=30-((a-50)/2);
    LineOut(c-50,49,c-30,53,DRAW_OPT_NORMAL);
    LineOut(c-10,49,c-30,53,DRAW_OPT_NORMAL);
    LineOut(c-10,49,c,55,DRAW_OPT_NORMAL);
    LineOut(c+10,49,c,55,DRAW_OPT_NORMAL);
    LineOut(c+5,51,c+15,55,DRAW_OPT_NORMAL);
    LineOut(c+25,51,c+15,55,DRAW_OPT_NORMAL);
    LineOut(c+20,51,c+35,55,DRAW_OPT_NORMAL);
    LineOut(c+40,49,c+35,55,DRAW_OPT_NORMAL);
    LineOut(c+40,51,c+55,55,DRAW_OPT_NORMAL);
    LineOut(c+60,49,c+55,55,DRAW_OPT_NORMAL);
    LineOut(c+60,51,c+85,55,DRAW_OPT_NORMAL);
    LineOut(c+100,49,c+85,55,DRAW_OPT_NORMAL);
    lines(a,b,1);
    if(abs(MotorRotationCount(OUT_A))>=15){
      a+=(MotorRotationCount(OUT_A))/100.0;
    }
    LineOut(c-10,49,c,55,DRAW_OPT_CLEAR);
    LineOut(c+10,49,c,55,DRAW_OPT_CLEAR);
    LineOut(c+5,51,c+15,55,DRAW_OPT_CLEAR);
    LineOut(c+25,51,c+15,55,DRAW_OPT_CLEAR);
    LineOut(c+20,51,c+35,55,DRAW_OPT_CLEAR);
    LineOut(c+40,49,c+35,55,DRAW_OPT_CLEAR);
    LineOut(c+40,51,c+55,55,DRAW_OPT_CLEAR);
    LineOut(c+60,49,c+55,55,DRAW_OPT_CLEAR);
    LineOut(c+60,51,c+85,55,DRAW_OPT_CLEAR);
    LineOut(c+100,49,c+85,55,DRAW_OPT_CLEAR);
    LineOut(c-50,49,c-30,53,DRAW_OPT_CLEAR);
    LineOut(c-10,49,c-30,53,DRAW_OPT_CLEAR);
    lines(a,b,0);
    if(rt==1){
      break;
    }
  }
  Wait(1500);
}



среда, 29 августа 2018 г.

Муравей учится ходить

Наш проект продолжает серию проектов, связанных с простейшими нейронными сетями и снова связан с самообучением. Робот-муравей ANTY, собранный по инструкции из книги Laurens Valk "The LEGO MINDSTORMS EV3 Discovery Book" будет обучаться ходить, не имея изначально ни малейшего представления о том, как это делать. Нейронная сеть робота будет оценивать попытки робота-муравья двигаться, оценивая их с точки зрения скорости движения и сохранения его направления и выстроит в итоге оптимальную взаимосвязь между движениями ног.


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


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


Нейронная сеть робота-муравья, которую мы будем обучать в этом проекте, будет действовать по следующему алгоритму:
  • Пара моторов, которые управляют движениями ног робота-муравья могут быть рассинхронизированы в диапазоне 0..360 градусов. Разобьем этот интервал на 36 отрезков. Это мы сделаем только из экономии времени на обучение сети. можно работать и с 360 различными углами.
  • Каждый из 36 возможных углов между ходовыми моторами свяжем с нейроном сети. Так как изначально нейронная сеть не обучена, каждый из вариантов синхронизации ног равноценен. Запишем в каждый нейрон начальный вес - 1.
  • Цикл обучения нейронной сети начинается со случайного выбора нейрона с учетом его веса.
  • Выбрав нейрон, синхронизируем ноги робота-муравья в соответствии с углом, связанным с выбранным нейроном
  • После синхронизации запоминаем расстояние до препятствия впереди и угол разворота на гироскопе
  • Делаем несколько шагов вперед
  • Оцениваем, изменился ли угол, если он изменился несущественно (робот не отклонился от курса), проверяем сократилось ли расстояние до препятствия и насколько
  • Если робот продвинулся вперед - увеличиваем вес нейрона 
  • Если робот не продвинулся вперед, то есть топчется на месте или пошел назад - уменьшаем вес нейрона, однако не менее чем до 1
  • Повторяем цикл обучения до тех пор, пока робот не будет выполнять шаги правильно и двигаться прямолинейно
Программу для робота мы написали на языке Python, выглядит она следующим образом:

Исходный код:
#!/usr/bin/env python3
from PIL import Image, ImageDraw, ImageFont
from ev3dev.ev3 import *
from time import sleep
import random
motorC = LargeMotor('outC')
motorB = LargeMotor('outB')
btn = Button()
summ=0
rand=0
stopc=0
stopb=0
motorB.reset()
motorC.reset()
maxn=0
def PID(B,C):
    trig=0
    triger=0
    es1 = 0
    es2 = 0
    u1 = 0
    u2 = 0
    Pk = 10
    PD = 20

    stopB = 0
    stopC = 0
    
    while(stopB!=1 or stopC!=1):
        if  motorB.position>=0:
            e1 = motorB.position%360 - B            
        else:
            e1 = -1*(abs(motorB.position)%360) - B
            
        if  motorC.position>=0:
            e2 = motorC.position%360 - C
        else:
            e2 = -1*(abs(motorC.position)%360) - C
             
        u1 = e1 * Pk + PD * (e1 - es1)
        u2 = e2 * Pk + PD * (e2 - es2)
        
        if(u1 > 900): u1 = 900
        if(u2 > 900): u2 = 900
        if(u1 < -900): u1 = -900
        if(u2 < -900): u2 = -900
        if abs(e1) <= 5:
            motorB.stop(stop_action='brake')
            stopB=1
        else:
            motorB.run_forever(speed_sp=u1*(-1))
      
        if abs(e2) <= 5:
            motorC.stop(stop_action='brake')
            stopC=1
        else:
            motorC.run_forever(speed_sp=u2*(-1))
        es1 = e1
        es2 = e2

gy = GyroSensor('in2') 
ir = InfraredSensor('in3') 
ir.mode = 'IR-PROX' 
motorC = LargeMotor('outC') 
motorB = LargeMotor('outB') 
ts = TouchSensor('in1')

brain = []
for i in range(36):
    brain.append(1)
while True:
    summ=0
    for i in range(36):
        summ += brain[i]
    rand = random.randint(0,summ*1000)/1000
    summ = 0
    for i in range(36):
       
        if summ < rand < summ + brain[i]:
            neuron = i
            break
        summ+=brain[i]
    sleep(1)

    PID(0,neuron*10)    
    inf=ir.value()
    gir=gy.value()
    sleep(1) 
    b=motorB.position
    c=motorC.position

    if(ir.value()>=10 and ir.value()<45):
        motorB.run_forever(speed_sp=200)
        motorC.run_forever(speed_sp=200)
        stopb=0
        stopc=0   
        while stopc==0 or stopb==0:
    
            if motorB.position>=b+360:
                stopb=1
            if motorC.position>=c+360:
                stopc=1
        motorB.stop(stop_action='brake')        
        motorC.stop(stop_action='brake')    
        if abs(gy.value()-gir)<=10:
             if  ir.value()-int(inf/2)>2:
                 brain[neuron]+=1
        else:
            brain[neuron]-=1
        if brain[neuron]<1:
            brain[neuron]=1
        print(neuron, brain[neuron]) 
        maxn=0
        max=0
        for i in range(len(brain)):
            if brain[i]>max:
                max=brain[i]
                maxn=i
        lcd = Screen()
        f =ImageFont.truetype('/usr/share/fonts/truetype/msttcorefonts/Arial.ttf', 75)
        lcd.draw.text((3,0), str(maxn), font=f)

        lcd.update()
    elif ir.value() <10:        
        motorB.run_forever(speed_sp= -400)
        motorC.run_forever(speed_sp= -400)
        stopb=0
        stopc=0
        while stopc==0 or stopb==0:    
            if motorB.position<=b-360:
                stopb=1
            if motorC.position<=c-360:
                stopc=1
        print("stop")
        motorB.stop(stop_action='brake')
        motorC.stop(stop_action='brake')
        if abs(gy.value()-gir)<=5:
            if ir.value()-int(inf/2):
                brain[neuron]+=1
        else:
            brain[neuron]-=3
        if brain[neuron]<1:
            brain[neuron]=1
        print(neuron, brain[neuron])
        maxn=0
        max=0
        for i in range(len(brain)):
            if brain[i]>max:
                max=brain[i]
                maxn=i
        lcd = Screen()
        f =ImageFont.truetype('/usr/share/fonts/truetype/msttcorefonts/Arial.ttf', 75)
        lcd.draw.text((3,0), str(maxn), font=f)

        lcd.update()

    if ir.value()>45:
      motorB.run_forever(speed_sp=300)
      motorC.run_forever(speed_sp=300)
    
    if(btn.backspace):
      motorB.stop()
      motorC.stop()
      exit()
    if(btn.enter):
      motorB.stop()
      motorC.stop()
      break
      
sleep(5)
PID(0,maxn*10)
while (not (btn.enter)): pass
      
print('dimonstracia')
for i in range(len(brain)):
    print(i,brain[i])
motorB.run_forever(speed_sp=200)
motorC.run_forever(speed_sp=200)
while (not (btn.enter)): pass
motorB.run_forever(speed_sp=-200)
motorC.run_forever(speed_sp=-200)

while (not (btn.backspace)):
    pass
motorB.stop()
motorC.stop()
exit()


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