суббота, 14 января 2017 г.

LEGO UART. Бит за битом

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


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


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


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



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

0 - светодиод выключена
1 - светодиод включен

Если нам нужно передать число большее 1, на помощь придет двоичный код. Посмотрите на таблицу ниже, число 2 будет представлено как 10, число 6 как 110, а число 12 как 1100.


Этот принцип сохраняется и для больших чисел, просто последовательности 0 и 1 будут длиннее. Для перевода из десятичного числа в двоичное есть несколько способов, например вот такой:


Итак, угол поворота ротора в двоичном виде мы получили, как теперь его передать? Светодиод у Правого робота и датчик освещенности у Левого позволяют нам создать такое распространенное устройство как Универсальный асинхронный приёмопередатчик (УАПП, англ. Universal Asynchronous Receiver-Transmitter, UART) — узел вычислительных устройств, предназначенный для организации связи с другими цифровыми устройствами. Преобразует передаваемые данные в последовательный вид так, чтобы было возможно передать их по одной физической цифровой линии другому аналогичному устройству. 
Передача данных в UART осуществляется по одному биту в равные промежутки времени. Этот временной промежуток определяется заданной скоростью UART и для конкретного соединения указывается в бодах (что в данном случае соответствует битам в секунду).

Для начала давайте проведем небольшой эксперимент, будем включать и выключать в цикле светодиод датчика и посмотрим за сколько выполнится 1000 таких переключений: В конструкции роботов мы использовали блоки LEGO NXT, для их программирования воспользуемся языком NXC:

int t;

task main()
{
  t = 0;
  while(t < 1000)
  {
    t = t + 1;
    SetSensorLight(IN_3,true);
    SetSensorLight(IN_3,false);
  }
  ClearScreen();
  NumOut(0, LCD_LINE4, CurrentTick());
  Wait(10000);
}

Запустив эту программу мы с удивлением обнаружили цифру 42000. Значит каждое полное переключение занимает целых 42000/1000 = 42 мс, а переключение из режима в режим вероятно около - 42/2 = 21 мс. Это довольно много, быстрый UART нам не создать. С другой стороны наш проект учебный, для нас главное - наглядность и понимание принципов работы устройства, гигабитных скоростей будем достигать совсем в других проектах :).

Теперь давайте напишем программу для Правого, передающего робота. 

// угол, на который повернут ротор у Правого робота
int a;
// длительность одного бита данных, мс
int t;
// массив для хранения числа a в двоичной форме
bool b[8];
// в этой переменной запоминаем горит ли светодиод на датчике
bool y;

task main()
{

  while(true)
  {
    // светодиод на датчике выключен
    y = false;
    // сохраняем в переменной а угол поворота ротора
    a = MotorRotationCount(OUT_B);
    // Это время в мс, в течении которого передается 1 бит информации
    t = 200;

    // ограничиваем а (угол поворота ротора) пределами 0..255
    if (a < 0)
    {
      a = 0;
    }
    // Мы будем передавать только 1 байт, это максимум 255
    if(a > 255)
    {
      a = 255;
    }

    // Очищаем экран и выводим на него a в десятичной форме
    ClearScreen();
    NumOut(0,13,a);

    // Переводим a в двочиный вид и сохраняем в массив b[8]    
    // выводим на экран также двоичиное пердставление числа
    for (int i=7; i >= 0; i--)
    {
      if (a / pow(2, i) >= 1)
      {
         b[i] = true;
         a = a - pow(2,i);
         NumOut((7-i)*8,0,1);
      }
      else
      {
         b[i] = false;
         NumOut((7-i)*8,0,0);
      }
    }

    // включаем светодиод и передаем стартовый бит
    SetSensorLight(IN_3,true);
    y = true;
    // Время передачи меньше на 21 мс, именно столько включается светодиод
    Wait(t-21);

    // передаем байт данных бит за битом мз массива b[8]
    for(int i = 7;i >= 0;i--)
    {
      if(b[i] == true)
      {
        SetSensorLight(IN_3,true);
        // длительность задержки различна, в зависимости от того горел ли светодиод
        if(y == true)
        {
          Wait(t-1);
        }
        else
        {
          Wait(t-21);
        }
        y = true;
      }
      else
      {
        SetSensorLight(IN_3,false);
        if(y == false)
        {
          Wait(t-1);
        }
        else
        {
          Wait(t-21);
        }
        y = false;
      }
    }

    // включаем светодиод и передаем стоповый, 10-й, бит
    SetSensorLight(IN_3,true);
    if(y == true)
    {
      Wait(t-1);
    }
    else
    {
      Wait(t-21);
    }
    y = true;

    // выключаем светодиод, готовимся к следующей итерации цикла
    SetSensorLight(IN_3,false);
    y = false;
    Wait(t*4);
  }
}

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

После того как число переведено в десятичную форму вступит в дело ПД-регулятор и развернет ротор принимающего робота в тоже положение, что и у передающего.

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

// принятое число в десятичной форме
int a, a_tmp;
// массив для хранения принятого числа в двоичной форме
bool b[8];
// длительность одного бита данных, мс
int t;
// управляющее воздействие ПД-регулятора
int u;
// пропорциональный коэтф-т ПД-регулятора
int pk;
// дифф.коэф-т ПД-регулятора
int dk;
// "старая" ошибка в дифф.компоненты ПД-регулятора
int es;
// данные с датчика
int D1;
// ошибка ПД-регулятора
int e;

// ПД-регулятор устанавливает ротор в положение a градусов
task motor()
{
  // значение коэф-тов ПД-реглятора
  pk = 2;
  dk = 4;
  es = 0;
  while(true)
  {
    // вычисляем ошибку (отклонение от целевой величины)
    e = a - MotorRotationCount(OUT_B);
    // формируем управляющее воздействие
    u = e*pk+dk*(e-es);
    // juhfybxbdftv управляющее воздействие диапазоном -100..100
    if(u < -100)
    {
      u = -100;
    }
    if(u > 100)
    {
      u = 100;
    }
    // опдаем управляющее воздействие на мотор
    OnFwd(OUT_B,u);
    // сохраняем значение ошибки
    es = e;
  }
}

task main()
{

  SetSensorLight(IN_3,false);
  a = 0;
  t = 200;
  start motor;
  while(true)
  {
    // ждем стартовый бит
    while(Sensor(IN_3)<65);
    // ждем первый бит данных
    Wait(t+t/2);

    // считываем все биты данных и записываем в массив b[8]
    for(int i = 7;i >= 0;i--)
    {
      if(Sensor(IN_3)>65)
      {
        b[i] = true;
        Wait(t);
      }
      else
      {
        b[i] = false;
        Wait(t);
      }
    }
    // очищаем экран
    ClearScreen();
    // если стоповый бит не получен - ошибка приема-передачи   
    if(Sensor(IN_3)<65) TextOut(0, LCD_LINE6, "ERROR");
    
    // выводим на экран принятое двоичное число
    for(int i = 7;i >= 0;i--)    
    {
      if(b[i] == true)
      {
         NumOut((7-i)*8,0,1);
      }
      else
      {
         NumOut((7-i)*8,0,0);
      }
    }

    // переводим число в десятичное
    a_tmp = 0;
    for(int i = 7;i >= 0;i--)
    {
      if(b[i] == true)
      {
        a_tmp = a_tmp + 1 * pow(2, i);

      }
    }
    a = a_tmp;
    // выводим десятичное число на экран
    NumOut(0,13,a);
    Wait(t*2);
  }
}

С учетом стартовых и стоповых битов скорость нашего UART составляет 4 бита/сек.

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





суббота, 7 января 2017 г.

Начинаем программировать EV3 на Python

Год назад мы перешли с графического программирование нашего робота LEGO Mindstorms EV3 на текстовое и первым языком программирования, который мы освоили, стал EV3 BASIC. Нам очень понравились его простота, отзывчивость к первым, зачастую неумелым попыткам выжать из него что-то большее, то, что мы могли получить на графическом "леговском" EV3-G. За этот год мы создали с использование BASICа целый ряд проектов (NAVIDOZ3R, Music Station, Телеграф, НУ ПОГОДИ, Саймон сказал), которые, как нам кажется, неплохо раскрыли возможности этого языка. Программы, написанные не нем работают существенно быстрее программ на "языке из кубиков". Бейсик не навязывает "хороший стиль" программирования, он дает свободу программисту - в этом его основная фишка.


Однако чем более сложными становились наши проекты с использованием EV3 BASIC, тем все более отчетливо мы ощущали его ограничения:

  • все переменные - глобальные
  • так же как и в EV3-G нет возможности работать с многомерными массивами без использования костылей
  • нельзя создать функцию с передачей в нее параметров и возвратом из нее результата
  • проблемы с поддержкой датчиков сторонних производителей

Хорошей альтернативой и следующей ступенькой в полноценном программировании EV3 традиционно считается ROBOTC, однако это достаточно дорогая ступенька, данная среда разработки далеко не бесплатна. Кроме этого ROBOC требует перепрошивки блока EV3 на свою прошивку, это не для всех приемлемо.
В мире opensource существует несколько свободных альтернатив для программирования платформы EV3 с использованием "взрослых" языков. Это LeJOS и язык Java, Monobrick с языком С# и ev3dev с целым рядом языков - Python, C, C++, JavaScript. Go.


Мы решили начать с ev3dev и языка Python, так как ходили слухи что язык этот имеет достаточно низкий порог вхождения подобно Бейсику, при этом не имея присущих ему недостатков. Еще на летних каникулах мы проштудировали замечательную детскую книгу по Python под названием "Hello World! Занимательное программирование" (Картер Сэнд, Уоррен Сэнд, 2016). На примере создания простейших игр книга действительно занимательно погружает читателя в основы языка Python.


Начав разбираться с ev3dev нам показалось, что информации из книги для погружения в Python не достаточно и в поисках ее дополнительных источников мы набрели на крутые онлайн-курсы на портале Stepic.org



В процессе прохождения первого курса, в попытках решения учебных задач с Python на EV3 пришло понимание того, что базовые навыки работы в ОС Linux пришлись бы юным робототехникам как нельзя кстати. Курс на том же Stepic под названием Введение в Linux оставил только положительные эмоции и дал массу полезной информации, в том числе и для размышления. 


Очень полезным источником информации для нас стал сайт по EV3 Python. Его автор, Nigel Ward, уже помогал нам год назад в адаптации EV3 BASIC для русскоязычных робототехников. Его материалы мы взяли за основу и в этот раз. Мы подготовили русскоязычное руководство по Python для EV3, которое состоит из двух частей:


В первой части - описание процесса установки и первоначальной настройки ev3dev. Во второй - справочник по "командам" и примеры программ на Python для платформы EV3.

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


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



1. Робот, поворачиваясь вокруг своей оси сканирует пространство, после чего выводит на экран "карту-радар" в виде лепестковой диаграммы.


# подключаем модуль поддержки оборудования EV3
from ev3dev.ev3 import *
# из модуля time будем использовать задержку - sleep
from time import sleep
# кроме это нам понадобится модуль математики
import math

# Это скорость разворота робота, в градусах/сек
speed = 50
# переменная для хранения значений с датчика-гироскопа
GyVal = 0

# создаем объект Gyro связанный с датчиком-гироскопа на 1 порту
Gyro = GyroSensor("in1")
# и объект IR, связанный с ИК-датчиком на порту 4
IR = InfraredSensor("in4")

# нам понадобится пара моторов, порты A и D
mA = LargeMotor('outA')
mD = LargeMotor('outD')

# устанавливаем гироскоп в режим измерения угла
Gyro.mode = "GYRO-ANG"

# создаем объект lcd для работы с экраном блока EV3
lcd = Screen()
# рисуем в буфере пару кругов (это скелет карты)
lcd.draw.ellipse(( 84, 59,  94, 69))
lcd.draw.ellipse(( 25, 0,  153, 128))
# выводим инфу из экранного буфера на экран
lcd.update()

# создаем список (массив) для хранения карты 
g = [0 for i in range(360)]

# запускаем робота на вращение вокруг своей оси
mA.run_forever(speed_sp=speed)
mD.run_forever(speed_sp=-1*speed)

# во время вращения заполняем массив g данными
# с ИК-датчика с привязкой у направлению (гироскоп)
while Gyro.value() < 360:
    GyVal =  abs(Gyro.value())    
    g[GyVal] = IR.value()*0.7

# после полного круга останавливаемся
mA.stop(stop_action="hold")
mD.stop(stop_action="hold")  

# выводим карту из массива в экранный буфер
# в виде лепестковой диаграммы
for i in range(90):
    lcd.draw.line(( 89, 64,  89+g[i]*math.sin(math.radians(i)), 64-g[i]*math.cos(math.radians(i))))
    lcd.draw.line(( 89, 64,  89+g[i+90]*math.cos(math.radians(i)), 64+g[i+90]*math.sin(math.radians(i))))
    lcd.draw.line(( 89, 64,  89-g[i+180]*math.sin(math.radians(i)), 64+g[i+180]*math.cos(math.radians(i))))
    lcd.draw.line(( 89, 64,  89-g[i+270]*math.cos(math.radians(i)), 64-g[i+270]*math.sin(math.radians(i))))

# выводим информацию из буфера на экран
lcd.update()
# издаем какой-то звук
Sound.beep()

# и замираем на 10 секунд чтобы успеть посмотреть карту
sleep(10)

2. Плавное изменение цветов и яркости подсветки по нажатию кнопок на блоке, например вверх-вниз изменяют яркость, влево-вправо изменяет цвет, нажатие в центр запускает автоматические переливы. Голосом озвучивается изменения режимов.

# подключаем модуль поддержки оборудования EV3
from ev3dev.ev3 import *
# из модуля time будем использовать задержку - sleep
from time import sleep

# объект для работы с кнопками на блоке  btn = Button()

tr = 1

# список цветов подсветки color = [Leds.RED, Leds.GREEN, Leds.AMBER, Leds.ORANGE, Leds.YELLOW]
cs = 0

# выключаем подсветку Leds.all_off()

# текущая яркость подсветки
on = 127

# цикла - пока не нажата кнопка назад while not btn.backspace:
    # если нажата кнопка влево
    if btn.check_buttons(buttons=['left']):
        cs=cs-1
        if cs<0:
            cs=4
        # проговариваем ее название
        # и изменяем цвет с выбранной яркостью         Sound.speak("Button left")
        Leds.set_color(Leds.LEFT, color[cs], on/255)
        Leds.set_color(Leds.RIGHT, color[cs], on/255)
        sleep(0.5)   

    # тоже самое для кнопки вправо     
elif btn.check_buttons(buttons=['right']):
        cs=cs+1
        if cs>4:
            cs=0

        Sound.speak("Button right")
        Leds.set_color(Leds.LEFT, color[cs], on/255)
        Leds.set_color(Leds.RIGHT, color[cs], on/255)
        sleep(0.5)

    # если нажата кнопка вверх
    if btn.check_buttons(buttons=['up']):
        # увеличиваем яркость подсветки
        on=on+10
        if on>240:
            Sound.speak("Maximum")
            on=255

        Leds.set_color(Leds.LEFT, color[cs], on/255)
        Leds.set_color(Leds.RIGHT, color[cs], on/255)
        sleep(0.1)

    # если нажата кнопка вниз
    elif btn.check_buttons(buttons=['down']):
        # уменьшаем яркость
        on=on-10
        if on<10:
            Sound.speak("Minimum")
            sleep(2)
            on=0
        
        Leds.set_color(Leds.LEFT, color[cs], on/255)
        Leds.set_color(Leds.RIGHT, color[cs], on/255)
        sleep(0.1)

    # при нажатии центральной кнопки     
if btn.check_buttons(buttons=['enter']):
        Sound.speak("Random color")
        tr = 1
        # цикл пока не нажата кнопка назад
        while not btn.backspace or tr!=0:
            Leds.set_color(Leds.RIGHT, Leds.RED,0)
            Leds.set_color(Leds.LEFT, Leds.GREEN,0)
            
            # "переливы" цветов и яркости подсветки
            for i in range(0,255,5):
                Leds.set_color(Leds.LEFT, Leds.GREEN,i/255)
                if btn.check_buttons(buttons=['backspace']):
                    tr = 0            
            if tr == 0:
                break                

            for i in range(255,0,-5):
                Leds.set_color(Leds.RIGHT, Leds.ORANGE,(255-i)/255) 
                Leds.set_color(Leds.LEFT, Leds.GREEN,i/255) 
                
                if btn.check_buttons(buttons=['backspace']) or tr == 0:
                    break              
        
            for i in range(0,255,5):
                Leds.set_color(Leds.LEFT, Leds.GREEN,i/255)
                if btn.check_buttons(buttons=['backspace']):
                    tr = 0
            if tr == 0:
                break
            Leds.set_color(Leds.RIGHT, Leds.ORANGE,1)

            for i in range(255,0,-5):
                Leds.set_color(Leds.RIGHT, Leds.ORANGE,i/255)
                Leds.set_color(Leds.LEFT, Leds.GREEN,i/255)

                if btn.check_buttons(buttons=['backspace']): 
                    break  
            for i in range(0,255,5):
                Leds.set_color(Leds.LEFT, Leds.YELLOW,i/255)
                if btn.check_buttons(buttons=['backspace']):
                    tr = 0
            if tr == 0:
                break

            for i in range(255,0,-5):
                Leds.set_color(Leds.RIGHT, Leds.RED,(255-i)/255)
                Leds.set_color(Leds.LEFT, Leds.YELLOW,i/255)

                if btn.check_buttons(buttons=['backspace']) or tr == 0:
                    break

            for i in range(0,255,5):
                Leds.set_color(Leds.LEFT, Leds.YELLOW,i/255)
                if btn.check_buttons(buttons=['backspace']):
                    tr = 0
            if tr == 0:
                break
            Leds.set_color(Leds.RIGHT, Leds.RED,1)

            for i in range(255,0,-5):
                Leds.set_color(Leds.RIGHT, Leds.RED,i/255)
                Leds.set_color(Leds.LEFT, Leds.YELLOW,i/255)

                if btn.check_buttons(buttons=['backspace']):
                    break

3. Робот должен проехать полный круг по линии и остановиться, определив и произнеся вслух диаметр круга, который на момент старта ему неизвестен. Гироскоп использовать нельзя.

# подключаем модуль для работы с EV3
from ev3dev.ev3 import *
# модуль для работы со временем
from time import *
# также нам потребуется pi из модуля математики
from math import pi

# объект для работы с кнопками на блоке
btn = Button()

# пара объектов для работы с моторами
mA = LargeMotor('outA')
mD = LargeMotor('outD')

# сбрасываем состояние энкодеров
# при старте программы они автоматически не сбрасываются!
mA.reset()
mD.reset()

# пара объектов для работы с датчиками освещенности
# у нас датчики от NXT 1.0, если будете использовать
# датчики цвета EV3, используйте класс ColorSensor
CS1 = LightSensor('in2')
CS2 = LightSensor('in3')

# режим работы датчиков - отраженный свет
# для EV3-датчиков цвета используйте 'COL-REFLECT' CS1.mode='REFLECT'
CS2.mode='REFLECT'

# средняя скорость движения по линии, градусов на энкодерах в сек speed = 800
# начальные сведения о радиусе круга - он неизвестен
r = 0

# параметры работы ПД-регулятора
u = 0
e = 0
es = 0
Pk = 1.1
Dk = 2

# создаем функцию для ограничения скорости моторов
# диапазоном -900..900 град/сек
def motor(speed):
    if speed>900:
        speed = 900
    elif speed<-900:
        speed = -900
    return speed

# цикл пока не нажата кнопка "назад"
while not btn.backspace:
    # рассчитываем ошибку для ПД-регулятора
    e = CS1.value() - CS2.value()
    # рассчитываем управляющее воздействие регулятора
    u = e*Pk + (e-es)*Dk
    # сохраняем ошибку для следующей итерации
    es = e

    # подаем управляющее воздействие на моторы
    # используя функцию, ограничивающую скорость  
    mA.run_forever(speed_sp=motor(speed+u))
    mD.run_forever(speed_sp=motor(speed-u))

    # рассчитываем радиус круга     
    if mD.position != mA.position:
        r = (61*(mA.position + mD.position))/(abs(mD.position-mA.position))
    # выводим на консоль текущий радиус круга
    print(r)   
    # небольшая задержка для работы регулятора
    sleep(0.01)
    # если проехали полный круг - вылет из цикла
    if (mA.position*(pi*56/360)+mD.position*(pi*56/360))/2 > pi*(r*2) and time()>3:
        break

# останавливаем моторы методом торможения
mA.stop(stop_action="hold")
mD.stop(stop_action="hold")

# произносим рассчитанный в процессе движения диаметр круга
Sound.speak('Stoped').wait()
Sound.speak(round(r*2)).wait()

4. Робот, имеющий в своей конструкции гироскоп, должен описать круг вокруг кегли и, вернувшись на место старта, остановиться.

# подключаем модуль для работы с EV3
from ev3dev.ev3 import *
# из модуля для работы со временем нам понадобится sleep
from time import sleep
# подключаем модуль математики
from math import *

# переменные для хранение данных с энкодеров
ma = 0
md = 0

# объект для работы с кнопками на блоке
btn = Button()
# средняя скорость робота, град/сек
speed = 200

# объекты для работы с моторами mA = LargeMotor('outA')
mD = LargeMotor('outD')

# объекты для работы с датчиками - инфракрасным и гироскопом IR = InfraredSensor('in4')
Gyro = GyroSensor("in1")

# устанавливаем режим работы гироскопа (измерение угла) Gyro.mode = 'GYRO-ANG'

# создаем функцию для ограничения скорости моторов
# диапазоном -900..900 град/сек
def motor(speed):
    if speed>900:
        speed = 900
    elif speed<-900:
        speed = -900
    return speed

# переменные для хранения расстояния и направления на ближайшую банку
MIN = 100
MINGRAD = 0

# в массиве g будем хранить данным с гироскопа
g = [0 for i in range(360)]

# вращаемся в поисках банки
mA.run_forever(speed_sp=speed)
mD.run_forever(speed_sp=-1*speed)

# находим ближайшую банку
while Gyro.value() < 359:
    GyVal =  Gyro.value()
    g[GyVal] = IR.value()*0.7

    if g[GyVal] < MIN:
        MIN = g[GyVal]
        MINGRAD = GyVal    

# останавливаемся
mA.stop(stop_action="hold")
mD.stop(stop_action="hold")

Sound.beep()

# расчет угла для поворота к банке боком
MINGRAD = MINGRAD + 90

# разворачиваемся к банке левым бортом
while abs(Gyro.value()-MINGRAD) > 1:
    speedL = -1*(Gyro.value()-MINGRAD)*5
    speedR = (Gyro.value()-MINGRAD)*5
    mA.run_forever(speed_sp=motor(speedL))
    mD.run_forever(speed_sp=motor(speedR))

# останавливаемся
mA.stop(stop_action="hold")
mD.stop(stop_action="hold")

sleep(2)

# сбрасываем гироскоп Gyro.mode = 'GYRO-RATE'
Gyro.mode = 'GYRO-ANG'

sleep(1)

grad = 0
# расчет длины стороны 360-ти угольника
i = 2*(MIN*6+70)*tan(radians(0.5))

# сброс энкодеров
mA.reset()
mD.reset()

# снижаем скорость до 50 граду/сек
speed = 50

# цикл - пока не проехали полный круг
# или не нажата кнопка
while grad > -360 and not btn.backspace:
    ob = 56*pi/360
    # если проехали 1 сторону 360-ти угольника
    if ((mA.position-ma)*ob+(mD.position-md)*ob)/2 > i:
        # поворачиваемся на 1 градус
        grad = grad-1
        ma = mA.position
        md = mD.position
    
    # пропорциональный регулятор от grad     
    speedL = speed-(Gyro.value()-grad)*16
    speedR = speed+(Gyro.value()-grad)*16

    # перед подачей на моторы ограничиваем скорость ф. motor()     
    mA.run_forever(speed_sp=motor(speedL))
    mD.run_forever(speed_sp=motor(speedR))

# останавливаемся после того как проехали круг
mA.stop(stop_action="hold")
mD.stop(stop_action="hold") 

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

# подключаем модуль для работы с EV3
from ev3dev.ev3 import *
# нам потребуются модули математики и времени
from math import *
import time

# объект для работы с кнопками на блоке
btn = Button()

# средняя скорость движения робота, град/сек
speed = 300

# объекты для работы с моторами
mA = LargeMotor('outA')
mD = LargeMotor('outD')

# объекты для работы с датчиками
LS1 = LightSensor('in2')
LS2 = LightSensor('in3')
# режим работы датчиков освещенности - отраженный свет
LS1.mode = 'REFLECT'
LS2.mode = 'REFLECT'

# объект для работы с гироскопом
Gyro = GyroSensor("in1")
# режим работы гироскопа - измерение угла
Gyro.mode = "GYRO-ANG"

# функция для ограничения скорости мотора (-900..900)
def motor(speed):    
    if speed>900:   
        speed = 900                             
    elif speed<-900:      
        speed = -900 
    return speed

# сброс энкодеров
mA.reset()
mD.reset()

# параметры ПД-регулятора
e = 0
es = 0
u = 0
Pk = 3
Dk = 6

# диаметр круга роботу неизвестен
D = 0

# пока не нажата кнопка назад
while not btn.backspace:
    e = LS1.value()/10 - LS2.value()/10
    u = e*Pk + (e-es)*Dk    
    es = e
      
    mA.run_forever(speed_sp=motor(speed+u))
    mD.run_forever(speed_sp=motor(speed-u))

    if mA.position != mD.position:
        D = (61*(mA.position + mD.position))/(abs(mA.position-mD.position))
    print(D)

    if (mA.position*(56*pi/360)+mD.position*(56*pi/360))/2 > pi*(D*2) and time.time() > 3:
        break

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

mA.stop(stop_action="hold")
mD.stop(stop_action="hold")
Sound.beep()

# сброс гироскопа
grad = 0
ma = 0
mb = 0
time.sleep(1)
Gyro.mode = 'GYRO-RATE'
Gyro.mode = 'GYRO-ANG'

speedL = 0
speedR = 0

ma = mA.position
mb = mD.position

tmp = D
speed = 50

# аналогично третьей задаче движемся по правильному
# 360-ти угольнику, но длину сторону уменьшаем 
# после каждой стороны, делая "круг" меньше
# как только диаметр вписанной окружности станет
# менее 1 см - останавливаемся

while not btn.backspace:
    ob = 56*pi/360
    if ((mA.position-ma)*ob+(mD.position-mb)*ob)/2 > (2*D)*tan(radians(0.5)):

        if mA.position > mD.position:
            grad = grad+1             
        else:
            grad = grad-1
        
        D = D-tmp/720
        ma = mA.position
        mb = mD.position

    speedL = speed-(Gyro.value()-grad)*10
    speedR = speed+(Gyro.value()-grad)*10

    mA.run_forever(speed_sp=motor(speedL))
    mD.run_forever(speed_sp=motor(speedR))

    print(D)
    if D<0.5:
        break

mA.stop(stop_action="hold")
mD.stop(stop_action="hold")
Sound.beep()

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