手柄udp遥控+姿态检测（双手举过鼻子时点亮电灯）

XEDU:

import cv2
import numpy as np
import pygame
import sys
import socket
from XEdu.hub import Workflow as wf

# 初始化 pygame
pygame.init()
pygame.joystick.init()

# 确保至少有一个手柄连接
if pygame.joystick.get_count() == 0:
    print("请连接一个Xbox手柄")
    sys.exit()

# 获取第一个手柄
joystick = pygame.joystick.Joystick(0)
joystick.init()

print(f"检测到手柄: {joystick.get_name()}")

# 设置屏幕大小
screen = pygame.display.set_mode((640, 480))
pygame.display.set_caption("Xbox手柄摇杆值显示")

# 设置UDP通信
esp32_left_ip = "192.168.1.100"   # 替换为左侧ESP32的IP地址
esp32_right_ip = "192.168.1.54"  # 替换为右侧ESP32的IP地址
esp32_port = 12345                # 替换为ESP32的端口号

sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

# 初始化视频捕捉
cap = cv2.VideoCapture(0)
body = wf(task='body17')  # 实例化pose模型
det = wf(task='bodydetect')  # 实例化detect模型

hands_up_prev = False  # 用于跟踪上一次的状态
# 设置窗口的初始大小
cv2.namedWindow('video', cv2.WINDOW_NORMAL)
cv2.resizeWindow('video', 1280, 1024)

# 创建字体对象
font = pygame.font.Font(None, 36)

# 初始化手柄摇杆变量
left_stick_x = 0.0
left_stick_y = 0.0
right_stick_x = 0.0
right_stick_y = 0.0

# 初始化 hands_up 变量
hands_up = False

# 主循环
running = True
while running and cap.isOpened():
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            running = False
        elif event.type == pygame.JOYAXISMOTION:
            # 获取左右摇杆的值
            left_stick_x = joystick.get_axis(0)
            left_stick_y = joystick.get_axis(1)
            right_stick_x = joystick.get_axis(2)  # 修改为2
            right_stick_y = joystick.get_axis(3)
            
            # 打印摇杆值
            print(f"左摇杆: X={left_stick_x:.3f}, Y={left_stick_y:.3f} | 右摇杆: X={right_stick_x:.3f}, Y={right_stick_y:.3f}")
            
            # 发送数据到ESP32
            left_stick_data = f"LX={left_stick_x:.3f},LY={left_stick_y:.3f}"
            right_stick_data = f"RX={right_stick_x:.3f},RY={right_stick_y:.3f}"
            
            sock.sendto(left_stick_data.encode(), (esp32_left_ip, esp32_port))
            sock.sendto(right_stick_data.encode(), (esp32_right_ip, esp32_port))

    ret, frame = cap.read()
    if not ret:
        break

    bboxs = det.inference(data=frame, thr=0.3)
    img = frame

    hands_up = False

    for i, bbox in enumerate(bboxs):
        keypoints, img = body.inference(data=img, img_type='cv2', bbox=bbox)

        # 计算鼻子位置
        nose_x, nose_y = keypoints[0][0], keypoints[0][1]

        # 计算两手高度
        left_hand_y = keypoints[9][2]
        right_hand_y = keypoints[10][3]

        # 判断是否两手都比鼻子高
        if left_hand_y < nose_y and right_hand_y < nose_y:
            hands_up = True

    for [x1, y1, x2, y2] in bboxs:  # 画检测框
        cv2.rectangle(img, (int(x1), int(y1)), (int(x2), int(y2)), (0, 255, 0), 2)

    if hands_up != hands_up_prev:  # 只有在状态改变时发送 UDP 消息
        if hands_up:
            udp_message = "hands_up:on"
        else:
            udp_message = "hands_up:off"
        sock.sendto(udp_message.encode(), (esp32_left_ip, esp32_port))  # 发送到左侧ESP32的IP和端口
        sock.sendto(udp_message.encode(), (esp32_right_ip, esp32_port))  # 发送到左侧ESP32的IP和端口
        hands_up_prev = hands_up  # 更新状态

    # 清屏
    screen.fill((0, 0, 0))

    # 显示摇杆数据
    left_text = font.render(f"left_stick: X={left_stick_x:.3f}, Y={left_stick_y:.3f}", True, (255, 255, 255))
    right_text = font.render(f"right_stick: X={right_stick_x:.3f}, Y={right_stick_y:.3f}", True, (255, 255, 255))
    screen.blit(left_text, (20, 20))
    screen.blit(right_text, (20, 60))

    # 显示 hands_up 状态
    hands_up_text = "hands_up:on" if hands_up else "hands_up:off"
    hands_up_display = font.render(hands_up_text, True, (255, 255, 255))
    screen.blit(hands_up_display, (20, 100))

    # 更新显示
    pygame.display.flip()

    cv2.imshow('video', img)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()
pygame.quit()
sock.close()
sys.exit()

esp32:

#include <WiFi.h>
#include <WiFiUdp.h>

const char* ssid = "42101";
const char* password = "15657859912";
const unsigned int localPort = 12345; // 与Python代码中的端口匹配
const int ledPin = 2;

WiFiUDP udp;

void setup() {
  pinMode(ledPin, OUTPUT);
  digitalWrite(ledPin, LOW);
  
  Serial.begin(115200);
  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.println("Connecting to WiFi...");
  }

  Serial.println("Connected to WiFi");
  udp.begin(localPort);
  Serial.println(WiFi.localIP());
  digitalWrite(ledPin, HIGH);
  delay(2000);
  digitalWrite(ledPin, LOW);
}

void loop() {
  int packetSize = udp.parsePacket();
  if (packetSize) {
    char incomingPacket[255];
    int len = udp.read(incomingPacket, 255);
    if (len > 0) {
      incomingPacket[len] = 0;
    }
    Serial.printf("Received packet: %s\n", incomingPacket);

    // 处理接收到的数据，例如解析 hands_up:on 或 hands_up:off
    if (strcmp(incomingPacket, "hands_up:on") == 0) {
      digitalWrite(ledPin, HIGH);  // 点亮 LED
    } else if (strcmp(incomingPacket, "hands_up:off") == 0) {
      digitalWrite(ledPin, LOW);   // 熄灭 LED
    }
  }
}