功能：通过摄像头采集姿势数据并存储到 SD 卡

import sensor, image, time, math, pyb, os

# 初始化摄像头
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)  # 320x240
sensor.skip_frames(time=2000)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)

# SD 卡配置
DATA_FILE = '/sd/motion_data.csv'
HEADER = 'timestamp,elbow_angle,knee_angle,valgus_ratio,status\n'

# 初始化数据文件
if DATA_FILE not in os.listdir('/sd'):
    with open(DATA_FILE, 'w') as f:
        f.write(HEADER)

def calculate_angle(a, b, c):
    """计算三点夹角（返回0-180度）"""
    ang_rad = math.atan2(c[1]-b[1], c[0]-b[0]) - math.atan2(a[1]-b[1], a[0]-b[0])
    ang_deg = math.degrees(ang_rad)
    return abs(ang_deg) if ang_deg > 0 else 360 + ang_deg

def detect_knee_valgus(hip, knee, ankle, img_width, img_height):
    """检测膝盖内扣（返回偏移比例）"""
    if ankle[0] == hip[0]:
        projection_x = hip[0]
    else:
        slope = (ankle[1] - hip[1]) / (ankle[0] - hip[0])
        projection_x = (slope*(knee[1] - hip[1]) + knee[0]) / (slope**2 + 1)
    horizontal_offset = (knee[0] - projection_x) / img_width
    hip_ankle_dist = math.hypot(ankle[0]-hip[0], ankle[1]-hip[1]) / img_width
    return abs(horizontal_offset) / hip_ankle_dist if hip_ankle_dist > 0.1 else 0

# 主循环
while True:
    img = sensor.snapshot()
    tags = img.find_apriltags(families=image.TAG36H11)
    
    if len(tags) >= 3:
        # 假设检测到的 AprilTag 分别代表髋、膝、踝
        hip = (tags[0].cx(), tags[0].cy())
        knee = (tags[1].cx(), tags[1].cy())
        ankle = (tags[2].cx(), tags[2].cy())
        
        # 计算特征
        elbow_angle = calculate_angle((hip[0], hip[1]-50), knee, ankle)
        valgus_ratio = detect_knee_valgus(hip, knee, ankle, img.width(), img.height())
        
        # 自动标注（示例规则：膝盖内扣比例>0.06或肘部角度异常则为error）
        status = "error" if valgus_ratio > 0.06 or elbow_angle < 90 or elbow_angle > 150 else "normal"
        
        # 保存数据
        timestamp = time.ticks_ms()
        with open(DATA_FILE, 'a') as f:
            f.write(f"{timestamp},{elbow_angle},{valgus_ratio},{status}\n")
    
    time.sleep_ms(100)  # 控制采样频率# Untitled - By: 25422 - Thu Mar 6 2025

import sensor
import time

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time=2000)

clock = time.clock()

while True:
    clock.tick()
    img = sensor.snapshot()
    print(clock.fps())