Привет! Я преподаю робототехнику и стараюсь делать учебные проекты интересными, вдохновляющими на изучение нового, с применением различных технологий и в то же время повторяемыми! В данной работа будет рассмотрено много аспектов робототехники, которые интересны моим ученикам, но могут быть полезны и остальным!
Сборка платформы
Механическая часть платформы самая простая — 4 колеса, и перфорированная платформа для удобства монтажа элементов управления для отладки.
Схема питания:
Используются Li-On аккумуляторы 18650. А для возможности их заряда не снимая с робота применяется плата балансировки заряда, а также модуль заряда, который подключается к Type-C и с 5В повышает напряжение до 8.4В, необходимых для заряда двух последовательно соединенных АКБ 18650.
Полный список компонентов для этого решения есть в посте в моем телеграм-канале.
Для управления логикой работы используетcя Arduino Nano в комплекте с радиомодулем NRF24L01.
Код для приемника:
#include
#include
#include
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
#include
#include
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()
Спасибо за внимание и интерес! Удачи и интересных экспериментов!
