PID小车用python代码写

import time
import RPi.GPIO as GPIO

# 设置GPIO模式
GPIO.setmode(GPIO.BOARD)

# 定义电机引脚
Motor1A = 16
Motor1B = 18
Motor2A = 22
Motor2B = 40

# 设置引脚为输出模式
GPIO.setup(Motor1A, GPIO.OUT)
GPIO.setup(Motor1B, GPIO.OUT)
GPIO.setup(Motor2A, GPIO.OUT)
GPIO.setup(Motor2B, GPIO.OUT)

# 定义PWM频率
pwm_frequency = 50

# 创建PWM实例
pwm1 = GPIO.PWM(Motor1A, pwm_frequency)
pwm2 = GPIO.PWM(Motor1B, pwm_frequency)
pwm3 = GPIO.PWM(Motor2A, pwm_frequency)
pwm4 = GPIO.PWM(Motor2B, pwm_frequency)

# 启动PWM
pwm1.start(0)
pwm2.start(0)
pwm3.start(0)
pwm4.start(0)

def set_motor_speed(speed):
    # 根据速度设置占空比
    duty_cycle1 = speed * 100 / pwm_frequency
    duty_cycle2 = speed * 100 / pwm_frequency
    duty_cycle3 = speed * 100 / pwm_frequency
    duty_cycle4 = speed * 100 / pwm_frequency

    # 设置占空比
    pwm1.ChangeDutyCycle(duty_cycle1)
    pwm2.ChangeDutyCycle(duty_cycle2)
    pwm3.ChangeDutyCycle(duty_cycle3)
    pwm4.ChangeDutyCycle(duty_cycle4)

try:
    while True:
        # 设置电机速度
        set_motor_speed(50)
        time.sleep(1)
        set_motor_speed(100)
        time.sleep(1)
        set_motor_speed(150)
        time.sleep(1)
except KeyboardInterrupt:
    # 停止PWM并清理GPIO
    pwm1.stop()
    pwm2.stop()
    pwm3.stop()
    pwm4.stop()
    GPIO.cleanup()