#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);

#define SERVOMIN  120 // 对应 0 度
#define SERVOMAX  490 // 对应 180 度

int angleToPulse(int ang) {
    return map(ang, 0, 180, SERVOMIN, SERVOMAX);
}

void setup() {
    Serial.begin(115200);
    Wire.begin(21, 22);
    
    pwm.begin();
    pwm.setPWMFreq(50); // 模拟舵机常用 50Hz
    
    // 初始化回中
    pwm.setPWM(0, 0, angleToPulse(90));
    pwm.setPWM(1, 0, angleToPulse(90));
}

void loop() {
    if (Serial.available() > 0) {
        String input = Serial.readStringUntil('\n');
        input.trim();
        
        int yIndex = input.indexOf('Y');
        int pIndex = input.indexOf('P');
        
        if (yIndex != -1 && pIndex != -1) {
            int yaw = input.substring(yIndex + 1, pIndex).toInt();
            int pitch = input.substring(pIndex + 1).toInt();
            
            // 物理限位保护，防止打死卡裂结构
            yaw = constrain(yaw, 15, 165);
            pitch = constrain(pitch, 30, 130);
            
            pwm.setPWM(0, 0, angleToPulse(yaw));
            pwm.setPWM(1, 0, angleToPulse(pitch));
        }
    }
}

import serial
import time
import math

class DesktopCompanion:
    def __init__(self, port='/dev/ttyUSB0', baudrate=115200):
        self.ser = serial.Serial(port, baudrate, timeout=1)
        time.sleep(2) # 等待 ESP32 复位完成
        self.current_yaw = 90
        self.current_pitch = 90

    def send_target(self, yaw: int, pitch: int):
        cmd = f"Y{int(yaw)}P{int(pitch)}\n"
        self.ser.write(cmd.encode('utf-8'))

    def idle_motion(self, duration=10):
        """让机器人在空闲状态下做平滑的头部微动"""
        start_time = time.time()
        print("开始空闲姿态循环...")
        
        while time.time() - start_time < duration:
            t = time.time() - start_time
            # 用正弦函数叠加制造不对称的运动轨迹
            target_yaw = 90 + 25 * math.sin(t * 0.8)
            target_pitch = 90 + 15 * math.cos(t * 1.2)
            
            self.send_target(target_yaw, target_pitch)
            time.sleep(0.02) # 50Hz 更新率

    def close(self):
        self.send_target(90, 90)
        self.ser.close()

if __name__ == '__main__':
    # 替换为你自己的串口端口，Windows 上一般为 COM3 / COM4 等
    robot = DesktopCompanion(port='/dev/ttyUSB0')
    try:
        robot.idle_motion(duration=15)
    except KeyboardInterrupt:
        pass
    finally:
        robot.close()