Блог Влада Иванова
[ПОМиРС 2] Собираем робота
Апр 3, 2020 в 14:24  •  42 мин  •  читали 863 раза

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


Первое, что нам нужно сделать это припаять провода к каждой двигательной сборке. Всего у нас будет по 5 проводов от каждого двигателя. 2 провода потолще идут на сам мотор, еще 3 идут на энкодер.



Зеленый провод - вывод C энкодера, его мы подключим к 3.3 в. Оранжевый и синий - это провода A и B, с них мы будем считывать состояние энкодера.


Собрем все на макетке и проверим работоспособность системы. Может уже сейчас можно сложить все добро в тазик и выкинуть с балкона:



Вот так все это выглядит, немного тестим - работает. Отлично, идем дальше.

(Прищепки нужны, правда)


Теперь нам нужно сделать некую основу для всего этого. Можно было бы скрутить провода вместе, но это не наш метод. Мы будем делать все по-правильному. Поэтому вытравим плату.


Изготовление печатной платы (ПП)


Cначала необходимо прикинуть, как это будет. Рисуем на бумаге:



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


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

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



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


Выглядит это как то так. Картинка слева - примерно 50% работы, справа - готовая плата.



Потом мы все это выпиливаем как можно более ровно



У меня получилось не очень ровно, поэтому придется еще немного обточить, а то некрасиво. Но это потом. Сейчас закинем нашу заготовку в старую сковородку, где уже разведен раствор хлорного железа. Возьмем воду 30-40 градусов по цельсию (тепло, которое вы уже ощущаете, однако оно не обжигает). Нужно брать 200-300 грамм вещества на 0.5 литров воды, но я беру на глаз потому что знаю, что должно получиться))))


Выглядит это не особо интересно:



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


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


Вот такая красота получается:



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



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


Тут уже припаяны компоненты и внешний вид с другой стороны таков:



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


Raspberry Pi просто вставляется в сокет, припаиваем все провода (для моторов и для энкодеров). Замываем флюс тем же ацетоном и вуаля:



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


После тестов робот оказался рабочим (ВАУ). Но отличий от первого теста особо нет, потому что программа пока не написана. Могу сказать, что моторы крутятся. Крутятся в нужные стороны, энкодеры читаются. А что еще нам нужно???


Теперь рассмотрим ПО


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



Мы рассмотрим класс Motor


Но для начала рассмотрим либу для управления скоростью моторов. Наш драйвер tb6612fng (красный на плате) имеет два входа, на которые мы можем подавать сигнал ШИМ для управления скоростью моторов.


Для этого мы будем использовать pi-blaster. Это полусофтварный pwm. Полу потому что используется DMA.

Вот тут можно узнать обо всем поподробнее: https://github.com/sarfata/pi-blaster/


Мы клонируем репу, собираем, делаем install и получаем сервис, который крутится в бэкграунде. Так же в директории /dev у нас появляется файлик pi-blaster, но это не обычный файл. Это FIFO буфер, в который мы пишем конструции типа 

<pwm_ch>=<value>

где value - число [0;1][0; 1]


Мы пишем, настройка применяется и мы имеем ШИМ сигнал. Вот такая для этого обертка:

%lang(python)%
import os

class PWM:
    def __init__(self, pin):
        self.pin = pin

    def set(self, value):
        cmd = 'echo \"' + str(self.pin) + '=' + str(value) + '\" > /dev/pi-blaster'
        os.system(cmd)


Теперь вернемся к классу Motor


Сначала импорты:

%lang(python)%
import RPi.GPIO as GPIO # либа для работы с GPIO RPi
from lib.pi_pwm.module import PWM # самописная либа, которую мы рассмотрели выше


Теперь давайте посмотрим на static методы класса


%lang(python)%
class Motor:
    @staticmethod
    def apply_requirements():
        GPIO.setmode(GPIO.BOARD)
        GPIO.setwarnings(False)
        GPIO.setup(22, GPIO.OUT)

    @staticmethod
    def lock_all():
        GPIO.output(22, False)

    @staticmethod
    def unlock_all():
        GPIO.output(22, True)


Метод apply_requirements нужен для установки начальных настроек. Мы говорим, что используем BOARD нотацию GPIO, говорим, что нас неинтересно видеть варнинги типа "Этот порт уже используется" и тд. Также мы устанавливаем 22 пин на выход. Этот пин будет подключен к STDBY ноге драйвера, которая отвечает за работу драйвера в принципе.


Соответственно, методы unlock_all и lock_all включают и выключают драйвер, а значит и моторы.


Идем дальше


Дальше мы видим конструктор класса

%lang(python)%
def __init__(self, config):
    self.__config__ = config
    self.__pwm_ch__ = PWM(self.__config__['pin_pwm'])
    self.__angle__ = 0.0
    self.__speed__ = 0.0

    self.__gpio_init__()
    self.__apply__(0)

    self.__prev_code__ = 0


В переменную конфиг передается конфиг типа такого:

%lang(python)%
config = {
	'log': True, # включаем логирование
	'm_a': 35, # первый пин мотора
	'm_b': 37, # второй пин мотора
	'en_a': 33, # первый пин энкодера
	'en_b': 31, # второй пин энкодера
	'pin_pwm': 27, # пин ШИМ
    'deg_per_tick': 37.5 # на сколько градусов поворачивается колесо, при 1 тике энкодера
}


Мы инициализируем канал PWM, устанавливаем скорость и угол 0, дальше инициализируем GPIO

%lang(python)%
def __gpio_init__(self):
    GPIO.setup(self.__config__['m_a'], GPIO.OUT)
    GPIO.setup(self.__config__['m_b'], GPIO.OUT)
    GPIO.setup(self.__config__['en_a'], GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
    GPIO.setup(self.__config__['en_b'], GPIO.IN, pull_up_down=GPIO.PUD_DOWN)


Входные ноги энкодера притягиваем к земле, чтоб они не болтались в воздухе. Пины мотора просто настраиваем на выход.


Дальше идет метод __apply__, он нужен для внутренней установки значений выходов и значений скважности ШИМ сигнала.

%lang(python)%
def __apply__(self, speed):
    self.__speed__ = speed
    if speed == 0:
        self.__pwm_ch__.set(0)
        GPIO.output(self.__config__['m_a'], False)
        GPIO.output(self.__config__['m_b'], False)
    elif speed > 0:
        self.__pwm_ch__.set(speed)
        GPIO.output(self.__config__['m_a'], True)
        GPIO.output(self.__config__['m_b'], False)
    else:
        self.__pwm_ch__.set(-1 * speed)
        GPIO.output(self.__config__['m_a'], False)
        GPIO.output(self.__config__['m_b'], True)


Теперь рассмотрим простые методы - геттеры, они просто возвращают приватные поля класса:

%lang(python)%
def get_angle(self):
    return self.__angle__

def get_speed(self):
    return self.__speed__

def get_config(self):
    return self.__config__


Команды мотору мы будем давать с помощью следующих методов:

%lang(python)%
def go(self, speed):
    if self.__config__['log']:
        print('[go] speed = ' + str(speed))
    self.__apply__(speed)

def stop(self):
    if self.__config__['log']:
        print('[stop]')
    self.__apply__(0)

def reset(self):
    if self.__config__['log']:
        print('[reset]')
    self.__angle__ = 0


go - устанавливает скорость мотора,

stop - останавливает мотор

reset - сбрасывает накопленный угол


И самое интересное:

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

Этот метод читает энкодеры и запоминает новый угол:


%lang(python)%
def __graydecode__(self, _gray):
    bin = 0
    gray = _gray
    while gray > 0:
        bin ^= gray
        gray >>= 1
    return bin

def update(self):
    en_a = GPIO.input(self.__config__['en_a'])
    en_b = GPIO.input(self.__config__['en_b'])
    code = self.__graydecode__(en_a | (en_b << 1))
    if code == 0:
        if self.__prev_code__ == 3:
            self.__angle__ += self.__config__['deg_per_tick']
        if self.__prev_code__ == 1:
            self.__angle__ -= self.__config__['deg_per_tick']
    self.__prev_code__ = code
    if self.__config__['log']:
        print('[update] angle = ' + str(self.__angle__))


Мы следим за входами энкодера, записываем в 0 бит значение входа а, в 1 бит значение входа b. Потом говорим мол это у нас код Грея, давайте преобразуем его в двоичный код.


Тут подробнее - https://en.wikipedia.org/wiki/Gray_code


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


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


Нет смысла опрашивать энкодер мильен раз в секунду, поэтому нужно будет сгородить ограничитель где то снаружи. Типа отпрашивать не чаще 1 мс, но это необязательно.


В общем то это все, на сегодня я уже достаточно много рассказал, ждите продолжения, первых тестов.

Спасибо за внимание!


Копирование материалов допускается только с разрешения автора (vladivanov.dev@gmail.com) в письменной форме.
(Copying of materials is allowed only with the written permission of the author)
Похожие статьи
ПОМиРС