'''
舵机驱动类
'''
from machine import Pin, PWM

class Servo:
    def __init__(self, pin):
        self.pwm = PWM(Pin(pin))
        self.pwm.freq(50)
        self.min_duty = 1638   # 0.5ms
        self.max_duty = 8192   # 2.5ms
        self.current_angle = 90
    
    def set_angle(self, angle):
        """设置角度 (0-180)"""
        angle = max(0, min(180, angle))
        duty = int(self.min_duty + (self.max_duty - self.min_duty) * angle / 180)
        self.pwm.duty_u16(duty)
        self.current_angle = angle
    
    def smooth_move(self, target, step=1, delay_ms=20):
        """平滑移动"""
        import time
        
        while self.current_angle != target:
            if self.current_angle < target:
                self.current_angle = min(self.current_angle + step, target)
            else:
                self.current_angle = max(self.current_angle - step, target)
            self.set_angle(self.current_angle)
            time.sleep_ms(delay_ms)
    
    def release(self):
        """释放舵机"""
        self.pwm.duty_u16(0)

# 使用
servo = Servo(16)
servo.set_angle(0)
time.sleep(1)
servo.smooth_move(180)

'''
直流电机 PWM 调速
'''
from machine import Pin, PWM

class DCMotor:
    def __init__(self, pin1, pin2):
        self.pwm1 = PWM(Pin(pin1))
        self.pwm2 = PWM(Pin(pin2))
        self.pwm1.freq(1000)
        self.pwm2.freq(1000)
    
    def forward(self, speed=100):
        """正转"""
        duty = int(speed / 100 * 65535)
        self.pwm1.duty_u16(duty)
        self.pwm2.duty_u16(0)
    
    def backward(self, speed=100):
        """反转"""
        duty = int(speed / 100 * 65535)
        self.pwm1.duty_u16(0)
        self.pwm2.duty_u16(duty)
    
    def stop(self):
        """停止"""
        self.pwm1.duty_u16(0)
        self.pwm2.duty_u16(0)
    
    def brake(self):
        """刹车"""
        self.pwm1.duty_u16(65535)
        self.pwm2.duty_u16(65535)

# 使用
motor = DCMotor(14, 15)
motor.forward(50)  # 50% 速度正转
time.sleep(2)
motor.backward(30)  # 30% 速度反转
time.sleep(2)
motor.stop()

'''
实验：130 风扇电机控制
'''
from machine import Pin, PWM

fan = PWM(Pin(14))
fan.freq(1000)

def set_fan_speed(percent):
    """设置风扇速度 (0-100%)"""
    duty = int(percent / 100 * 65535)
    fan.duty_u16(duty)

# 风速等级
speeds = [0, 30, 60, 100]

for speed in speeds:
    print(f"风速: {speed}%")
    set_fan_speed(speed)
    time.sleep(3)

'''
实验：温控风扇
'''
from machine import ADC

temp_sensor = ADC(4)

def read_temperature():
    raw = temp_sensor.read_u16()
    voltage = raw * 3.3 / 65535
    return 27 - (voltage - 0.706) / 0.001721

while True:
    temp = read_temperature()
    
    # 温度-速度映射
    if temp < 30:
        speed = 0
    elif temp < 35:
        speed = 30
    elif temp < 40:
        speed = 60
    else:
        speed = 100
    
    set_fan_speed(speed)
    print(f"温度: {temp:.1f}°C  风速: {speed}%")
    time.sleep(2)

'''
实验：简易机械臂
'''
class RobotArm:
    def __init__(self, base_pin, shoulder_pin, elbow_pin, gripper_pin):
        self.base = Servo(base_pin)
        self.shoulder = Servo(shoulder_pin)
        self.elbow = Servo(elbow_pin)
        self.gripper = Servo(gripper_pin)
        self.home()
    
    def home(self):
        """归位"""
        self.base.set_angle(90)
        self.shoulder.set_angle(90)
        self.elbow.set_angle(90)
        self.gripper.set_angle(90)
    
    def grab(self):
        """抓取"""
        self.gripper.smooth_move(30)
    
    def release(self):
        """释放"""
        self.gripper.smooth_move(90)
    
    def pick_and_place(self, from_pos, to_pos):
        """拾取放置"""
        # 移动到拾取位置
        self.base.smooth_move(from_pos[0])
        self.shoulder.smooth_move(from_pos[1])
        self.elbow.smooth_move(from_pos[2])
        
        # 抓取
        self.grab()
        time.sleep(0.5)
        
        # 移动到放置位置
        self.shoulder.smooth_move(90)  # 抬起
        self.base.smooth_move(to_pos[0])
        self.shoulder.smooth_move(to_pos[1])
        self.elbow.smooth_move(to_pos[2])
        
        # 释放
        self.release()

# 使用
arm = RobotArm(16, 17, 18, 19)
arm.pick_and_place((45, 60, 120), (135, 60, 120))