Привет! Я преподаю робототехнику и стараюсь делать учебные проекты интересными, вдохновляющими на изучение нового, с применением различных технологий и в то же время повторяемыми! В данной работа будет рассмотрено много аспектов робототехники, которые интересны моим ученикам, но могут быть полезны и остальным!
Сборка платформы
Механическая часть платформы самая простая — 4 колеса, и перфорированная платформа для удобства монтажа элементов управления для отладки.
Схема питания:
Используются Li-On аккумуляторы 18650. А для возможности их заряда не снимая с робота применяется плата балансировки заряда, а также модуль заряда, который подключается к Type-C и с 5В повышает напряжение до 8.4В, необходимых для заряда двух последовательно соединенных АКБ 18650.
Полный список компонентов для этого решения есть в посте в моем телеграм-канале.
Для управления логикой работы используетcя Arduino Nano в комплекте с радиомодулем NRF24L01.
Код для приемника:
#include <SPI.h> #include <nRF24L01.h> #include <RF24.h> const uint64_t pipe = 0xF0F0F0F0F0LL; RF24 radio(9, 10); // CE, CSN byte data[1]; uint32_t radioTimer=0; int speed = 128; void setup() { Serial.begin(9600); Serial.println(!radio.begin()); delay(2); radio.setChannel(100); // канал (0-127) radio.setDataRate(RF24_1MBPS); radio.setPALevel(RF24_PA_HIGH); radio.openReadingPipe(1, pipe); radio.startListening(); pinMode(2,OUTPUT); pinMode(3,OUTPUT); pinMode(4,OUTPUT); pinMode(5,OUTPUT); } void forward() { digitalWrite(2,0); analogWrite(3,speed); digitalWrite(4,0); analogWrite(5,speed); } void backward() { digitalWrite(2,1); analogWrite(3,255-speed); digitalWrite(4,1); analogWrite(5,255-speed); } void left() { digitalWrite(2,0); analogWrite(3,0); digitalWrite(4,0); analogWrite(5,speed); } void right() { digitalWrite(2,0); analogWrite(3,speed); digitalWrite(4,0); analogWrite(5,0); } void STOP(){ digitalWrite(2,0); analogWrite(3,0); digitalWrite(4,0); analogWrite(5,0); } void loop() { if (radio.available()) { radioTimer = millis(); radio.read(data,1); byte p1 = (data[0] >> 0) & 1; byte p2 = (data[0] >> 1) & 1; byte p3 = (data[0] >> 2) & 1; byte p4 = (data[0] >> 3) & 1; if (p1 && p2 && p3 && p4) forward(); else if (p1 && !p2 && !p3 && p4) backward(); else if (p1 && !p2 && !p3 && !p4) left(); else if (!p1 && !p2 && !p3 && p4) right(); else if (!p1 && !p2 && !p3 && !p4) STOP(); } if (millis()-radioTimer>500) STOP(); } В целом код достаточно прост, однако некоторые моменты прокомментирую:
-
Подключение библиотек и определение констант и переменных:
-
Подключаются библиотеки для работы с SPI-интерфейсом и nRF24L01.
-
Устанавливаются номера пинов для управления модулем RF24.
-
Определяется адрес трубы связи для приёма данных.
-
Объявляются переменные для хранения данных и таймера радио.
-
-
Настройки в функции
setup():-
Инициализация Serial порта для отладки.
-
Настройка параметров радиомодуля, таких как канал связи, скорость передачи данных и уровень мощности передатчика.
-
Конфигурация пинов для управления двигателями.
-
-
Функции управления движением:
-
forward(),backward(),left(),right(),STOP(): функции для управления двигателями в различных направлениях или остановки устройства.
-
-
Основной цикл в
loop():-
Проверка наличия данных от радиопередатчика.
-
Чтение и интерпретация полученных данных для управления движениями устройства. Данные передаются в одном байте, поэтому используются операции битового сдвига, запись в 4 отдельные переменные для простоты понимания и дальнейшей работы с управлением
-
Автоматическая остановка устройства, если в течение 500 мс не было получено новых команд.
-
Передатчик
Любая Arduino + радиомодуль NRF24L01.
Задача этого устройства: получать данные от скрипта, работающего с камерой и передавать их на мобильную платформу.
Программа для этой части:
#include <SPI.h> #include <nRF24L01.h> #include <RF24.h> const uint64_t pipe = 0xF0F0F0F0F0LL; long timer; RF24 radio(9, 10); // CE, CSN byte send[1] = {0}; void setup() { Serial.begin(9600); Serial.println(radio.begin()); delay(2); radio.setChannel(100); radio.setDataRate(RF24_1MBPS); radio.setPALevel(RF24_PA_HIGH); radio.setAutoAck(1); radio.stopListening(); radio.openWritingPipe(pipe); } void loop() { if (Serial.available() > 0) { send[0] = Serial.read(); radio.write(send, 1); } }-
Подключение библиотек и определение констант и переменных:
-
Подключаются библиотеки для работы с SPI-интерфейсом и nRF24L01.
-
Устанавливаются номера пинов для управления модулем RF24.
-
Определяется адрес и номер канала связи для приёма данных. (ВАЖНО, чтобы они совпадали на передатчике и приемнике)
-
Объявляются переменные для хранения данных и таймера радиопередатчика.
-
-
Настройки в функции
setup():-
Инициализация Serial порта
-
Настройка параметров радиомодуля, таких как канал связи, скорость передачи данных и уровень мощности передатчика.
-
-
Основной цикл в
loop():-
Проверка наличия данных от Python-скрипта через Serial.
-
Передача полученного байта через радиоканал на платформу
-
Обработка жестов руки
Для обработки используются библиотеки mediapipe (для распознавания точек) и OpenCV для визуализации изображения.
Устанавливаются они стандартной командой pip (или pip3 для linux):
pip install mediapipe pip install opencv-pythonПолучение ключевых точек руки происходит в несколько команд:
import cv2 import mediapipe as mp import numpy as np mp_hands = mp.solutions.hands mp_drawing = mp.solutions.drawing_utils hands = mp_hands.Hands(static_image_mode=False, max_num_hands=1) cap = cv2.VideoCapture(0) while True: ret, frame = cap.read() if not ret: continue frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) results = hands.process(frame) frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR) if results.multi_hand_landmarks: for hand_landmarks in results.multi_hand_landmarks: mp_drawing.draw_landmarks(frame, hand_landmarks, mp_hands.HAND_CONNECTIONS) cv2.imshow('Fingers', frame) if cv2.waitKey(10) == 27: break cap.release() cv2.destroyAllWindows() Этот код открывает камеры, читает поток изображений и передает его в обработку библиотеке MediaPipe. Важными параметрами являются:
-
static_image_mode=False — гарантирует, что при потоковом видео будет постоянно определяться одна и та же рука
-
max_num_hands=1 — исключает обработку других найденных в кадре рук.
hands = mp_hands.Hands(static_image_mode=False, max_num_hands=1)В результате получаем картинку:
Следующим шагом необходимо пронумеровать все маркеры на руке, чтобы можно было выделить ключевые точки каждого пальца.
Далее, определяем расстояние между крайними точками каждого пальца, и если они меньше заданного порога, считает что палец загнут.
import cv2 import mediapipe as mp import numpy as np mp_hands = mp.solutions.hands mp_drawing = mp.solutions.drawing_utils hands = mp_hands.Hands(static_image_mode=False, max_num_hands=1) cap = cv2.VideoCapture(0) tip_ids = [4, 8, 12, 16, 20] base_ids = [0, 5, 9, 13, 17] extension_threshold = 0.17 def get_vector(p1, p2): return np.array([p2.x - p1.x, p2.y - p1.y, p2.z - p1.z]) def is_finger_extended(base, tip, is_thumb=False): base_to_tip = get_vector(base, tip) base_to_tip_norm = np.linalg.norm(base_to_tip) return base_to_tip_norm > extension_threshold while True: ret, frame = cap.read() if not ret: continue frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) results = hands.process(frame) frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR) if results.multi_hand_landmarks: for hand_landmarks in results.multi_hand_landmarks: mp_drawing.draw_landmarks(frame, hand_landmarks, mp_hands.HAND_CONNECTIONS) if hand_landmarks: landmarks = hand_landmarks.landmark for id, landmark in enumerate(hand_landmarks.landmark): h, w, c = frame.shape cx, cy = int(landmark.x * w), int(landmark.y * h) cv2.putText(frame, str(id), (cx, cy), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 1) for finger_index, tip_id in enumerate(tip_ids): base_id = base_ids[finger_index] if is_finger_extended(landmarks[base_id], landmarks[tip_id]): cx, cy = int(landmarks[tip_id].x * frame.shape[1]), int(landmarks[tip_id].y * frame.shape[0]) cv2.circle(frame, (cx, cy), 10, (0, 255, 0), cv2.FILLED) cv2.imshow('Fingers', frame) if cv2.waitKey(10) == 27: break cap.release() cv2.destroyAllWindows() Из-за особенностей строения метод определения расстояния между крайними точками не подходит для большого пальца. Поэтому в этом проекте (чтобы не усложнять) оставим эту мысль.
Итак, у нас есть 4 пальца для управления и сжатая рука для остановки робота:
Остается преобразовать состояние пальцев в биты, сложить их в один байт и передать в Arduino.
Полный код проекта на Python
import cv2 import mediapipe as mp import numpy as np import serial import serial.tools.list_ports import time ser = serial.Serial("COM11", 9600, timeout=1) if ser is None: exit() # Завершаем программу, если подключение не удалось time.sleep(2) #Ждем открытия порта # Переменная для хранения состояний светодиодов handStates = 0 mp_hands = mp.solutions.hands mp_drawing = mp.solutions.drawing_utils hands = mp_hands.Hands(static_image_mode=False, max_num_hands=1, min_detection_confidence=0.7) tip_ids = [4, 8, 12, 16, 20] # Индексы кончиков пальцев base_ids = [0, 5, 9, 13, 17] # Индексы баз пальцев cap = cv2.VideoCapture(0) extension_threshold = 0.17 # Общий порог для большинства пальцев thumb_extension_threshold = 0.1 # Специальный порог для большого пальца def get_vector(p1, p2): """ Возвращает вектор от точки p1 к точке p2 """ return np.array([p2.x - p1.x, p2.y - p1.y, p2.z - p1.z]) def is_finger_extended(base, tip, is_thumb=False): """ Определяет, разогнут ли палец, исходя из его вектора """ base_to_tip = get_vector(base, tip) # Нормализация вектора base_to_tip_norm = np.linalg.norm(base_to_tip) # Проверка на разгибание, учитывая, является ли это большим пальцем if is_thumb: return base_to_tip_norm > thumb_extension_threshold else: return base_to_tip_norm > extension_threshold def count_fingers(hand_landmarks): finger_count = 0 extended_fingers = [] finger_states = [0, 0, 0, 0, 0] # Состояние пальцев: 0 - сжат, 1 - разогнут if hand_landmarks: landmarks = hand_landmarks.landmark # Проверка большого пальца с учетом его специфики ## if is_finger_extended(landmarks[base_ids[0]], landmarks[tip_ids[0]], is_thumb=True): ## finger_count += 1 ## extended_fingers.append(tip_ids[0]) ## finger_states[0] = 1 # Проверка остальных пальцев for i in range(1, 5): if is_finger_extended(landmarks[base_ids[i]], landmarks[tip_ids[i]]): finger_count += 1 extended_fingers.append(tip_ids[i]) finger_states[i] = 1 return finger_count, extended_fingers, finger_states while True: ret, frame = cap.read() if not ret: continue frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) results = hands.process(frame) frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR) if results.multi_hand_landmarks: for hand_landmarks in results.multi_hand_landmarks: mp_drawing.draw_landmarks(frame, hand_landmarks, mp_hands.HAND_CONNECTIONS) fingers_counted, extended_fingers, finger_states = count_fingers(hand_landmarks) # Подсветка кончиков разогнутых пальцев for tip_index in extended_fingers: tip_landmark = hand_landmarks.landmark[tip_index] x, y = int(tip_landmark.x * frame.shape[1]), int(tip_landmark.y * frame.shape[0]) cv2.circle(frame, (x, y), 10, (0, 255, 0), cv2.FILLED) # Вывод состояния каждого пальца finger_state_text = ' '.join(['1' if state else '0' for state in finger_states]) cv2.putText(frame, f'Fingers: {finger_state_text}', (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2, cv2.LINE_AA) # Передаем значение пальцев в Arduino handStates = 0 for i in range(len(finger_states[1:])): handStates ^= (finger_states[i+1] << i) ser.write(bytearray([handStates])) cv2.imshow('Fingers Count', frame) if cv2.waitKey(10) & 0xFF == 27: break cap.release() cv2.destroyAllWindows() Спасибо за внимание и интерес! Удачи и интересных экспериментов!
ссылка на оригинал статьи https://habr.com/ru/articles/812863/
Добавить комментарий