树莓派4路电机速度

 

 

 

 

 

 

 

 

pip install opencv-python adafruit-circuitpython-pca9685 RPi.GPIO numpy

 

代码说明

 

1. PWM 控制:

   • 通过 PCA9685 控制 L298N 的 ENA/ENB，调整 PWM 占空比 来控制电机速度。
   • 速度范围 0-100%（代码中自动转换为 16-bit）。

2. 电机方向控制:

   • 通过树莓派 GPIO 输出 控制 L298N 的 IN1 ~ IN8。
   • GPIO.HIGH / GPIO.LOW 设置 前进/后退/停止。

3. 四种运动模式:

   • move_forward(speed): 让 4 个电机 前进。
   • move_backward(speed): 让 4 个电机 后退。
   • turn_left(speed): 让 左侧电机后退，右侧电机前进。
   • turn_right(speed): 让 右侧电机后退，左侧电机前进。
   • stop(): 停止所有电机。

4. 运行测试:

   • 前进 2 秒
   • 后退 2 秒
   • 左转 2 秒
   • 右转 2 秒
   • 停止

硬件连接

 

 

main.py

import cv2
import socket
import struct
import pickle
import threading

import time

from API_CAR import *



    
    
    
# 视频流服务器
def video_stream_server():
    server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    server.bind(('0.0.0.0', 5002))
    server.listen(5)
    print("视频服务器启动，等待连接...")
    client, addr = server.accept()
    print(f"视频客户端连接：{addr}")
 
    cap = cv2.VideoCapture(0)
     
    while True:
        success, frame = cap.read()
        if not success:
            break
        frame = cv2.resize(frame, (640, 480))  # 降低分辨率
        _, buffer = cv2.imencode('.jpg', frame, [cv2.IMWRITE_JPEG_QUALITY, 50])  # JPEG 压缩
        data = pickle.dumps(buffer)
        client.sendall(struct.pack("Q", len(data)) + data)  # 发送数据大小+压缩图像
        time.sleep(0.05)  # 控制帧率 (20FPS)
 
    cap.release()
    client.close()
 
# 控制指令服务器
def control_server():
    global current_speed
    server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    server.bind(('0.0.0.0', 5003))
    server.listen(5)
    print("控制服务器启动，等待连接...")
 
    while True:
        client, addr = server.accept()
        print(f"控制客户端连接：{addr}")
        while True:
            try:
                data = client.recv(1024).decode().strip()
                if not data:
                    break
                print("Rec data:",data)
                if data.startswith("FORWARD"):
                    _, speed = data.split()
                    current_speed = int(speed)
                    print(f"FORWARD  速度设置为 {current_speed}%")
                    Car_qian(current_speed)
                    time.sleep(3)
                    Car_stop()
                    
                elif data.startswith("BACKWARD"):
                    _, speed = data.split()
                    current_speed = int(speed)
                    print(f"BACKWARD  速度设置为 {current_speed}%")
                    Car_hou(current_speed)
                    time.sleep(3)
                    Car_stop()
                    
                elif data.startswith("RIGHTMOVE"):
                    _, speed = data.split()
                    current_speed = int(speed)
                    print(f"RIGHTMOVE  速度设置为 {current_speed}%")
                    Car_move_right(current_speed)
                    time.sleep(1)
                    Car_stop()                    
                elif data.startswith("LEFTMOVE"):
                    _, speed = data.split()
                    current_speed = int(speed)
                    print(f"LEFTMOVE  速度设置为 {current_speed}%")
                    Car_move_left(current_speed)
                    time.sleep(1)
                    Car_stop()                     
                    
                elif data.startswith("LEFT"):
                    _, speed = data.split()
                    current_speed = int(speed)
                    print(f"LEFT  速度设置为 {current_speed}%")
                    Car_turn_left(current_speed)
                    time.sleep(3)
                    Car_stop()
                elif data.startswith("RIGHT"):
                    _, speed = data.split()
                    current_speed = int(speed)
                    print(f"RIGHT  速度设置为 {current_speed}%")
                    Car_turn_right(current_speed)
                    time.sleep(3)
                    Car_stop()
                    

                elif data.startswith("ALLSPEED"):
                    _, speed = data.split()
                    current_speed = int(speed)
                    print(f"速度设置为 {current_speed}%")
                    set_motor_allspeed(current_speed)
                    time.sleep(3)
                    Car_stop()
                elif data == "STOP":
                    #time.sleep(3)
                    Car_stop()
                    pass
                    
                elif data.startswith("yaw"):
                    _, speed = data.split()
                    
                    print(f"yaw 设置为 {speed}%")     
                    set_servo_angle(Dgree_pin['yaw'], int(speed))
                    
                elif data.startswith("roll"):
                    _, speed = data.split()
                    
                    print(f"roll 设置为 {speed}%")     
                    
                    set_servo_angle(Dgree_pin['roll'], int(speed))

                elif data.startswith("pitch"):
                    _, speed = data.split()
                    
                    print(f"pitch 设置为 {speed}%")     
                                       
                    set_servo_angle(Dgree_pin['pitch'], int(speed))
    
                #stop()       
                client.send(b"OK")
            except:
                break
        client.close()
 
if __name__ == "__main__":
    threading.Thread(target=video_stream_server, daemon=True).start()
    threading.Thread(target=control_server, daemon=True).start()
 
    while True:
       
        time.sleep(3)
        
       

　　API_CAR.py

import time
import board
import busio
import RPi.GPIO as GPIO
from Adafruit_PCA9685 import PCA9685


# 树莓派 GPIO 设置
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)

#SCL (时钟线)：GPIO 3（物理引脚 5）
#SDA (数据线)：GPIO 2（物理引脚 3）    
#=================1==========================  
# 初始化 I2C 连接
i2c = busio.I2C(board.SCL, board.SDA)

if not i2c.try_lock():
    print("I2C 未能成功初始化！")
    
else:
    try:
        print("I2C 初始化成功！")

    finally:
        i2c.unlock()  # 释放 I2C 锁

Dgree_pin ={'yaw':4,'roll':5,'pitch':6}
# 设置 PWM 频率为 50Hz（舵机标准）
#使用 树莓派 + PCA9685 模拟四旋翼无人机 遥控器（RC）PWM 信号。
#输出 四个通道的 PWM 信号，分别对应 油门、偏航、俯仰、横滚（Throttle, Yaw, Pitch, Roll）。
#PWM 范围：1000us（最小油门）~ 2000us（最大油门）（典型遥控器信号）。
#更新 PWM 频率为 50Hz（标准 RC 频率）。

pca = PCA9685()
pca.set_pwm_freq(50)
pca.frequency = 50  # 设置PWM频率（1000Hz 适用于L298N）


# 4️⃣ 计算 PWM 幅度
def set_servo_angle(channel, angle):
    #信号是自己程序伪造的飞行器接收机，因此实际度数映射有偏差
    #这个度数只是个参考值 每个轴旋转范围不一样 需要实际重新角度映射
    #默认45时候都在水平中心，偏航上电前需要手动把头朝着正前才是默认基准位置。
    #偏航大概 20-160  俯仰大概35-60 横滚 40-50 实际具体测试边界
    # 举个例子  假设默认上电 是45度 给了35度 实际旋转了30度（理论上应该是10度），这里需要重新映射，【30-45】-【10-0】
    print(channel, channel)
    # 3️⃣ 设置舵机的 PWM 范围
    SERVO_MIN_PULSE = 1000   # 1000us → 0° 不能改 
    SERVO_MAX_PULSE = 2000  # 2000us → 180° 不能改 
    SERVO_RANGE = 180.0       # 舵机角度范围（0° - 180°） 能改
    
    #  

    """使用 set_pwm 设置舵机角度"""
    pulse_length = 1000000 / pca.frequency  # 计算 PWM 总周期（微秒）
    pulse_length //= 4096  # PCA9685 12-bit 分辨率，每步 = 总周期 / 4096

    # 计算脉冲宽度（单位：4096 级）
    pulse = int((angle / SERVO_RANGE) * (SERVO_MAX_PULSE - SERVO_MIN_PULSE) + SERVO_MIN_PULSE)
    pulse //= pulse_length

    # 使用 set_pwm 设置 PWM (0=起始点, pulse=结束点)
    pca.set_pwm(channel, 0, int(pulse))
    
    
    #speed_percentage = max(-100, min(100, angle))  # 限制速度范围 0% 到 100%
    #duty_cycle = int(abs(speed_percentage) / 100 * 4095)  # PCA9685 使用 12-bit (0-4095)
    
    #pca.set_pwm(channel, 0, int(pulse))
#set_servo_angle(6,60)  
#================2===========================    
    


# L298N 连接设置
motor_pins = {
    "motor1_zuoqian": {"in1": 16, "in2": 12, "pwm": 0},
    "motor2_youqian": {"in1": 21, "in2": 20, "pwm": 1},
    "motor3_zuohou": {"in1": 26, "in2": 19, "pwm": 2},
    "motor4_youhou": {"in1": 13, "in2": 6, "pwm": 3}
}

current_speed = 50



# 初始化 GPIO
for motor, pins in motor_pins.items():
    GPIO.setup(pins["in1"], GPIO.OUT)
    GPIO.setup(pins["in2"], GPIO.OUT)


def set_motor_allspeed(speed_percentage):
    print("设置所有速度")
    for i_name in motor_pins:
        pins = motor_pins[i_name]
        speed_percentage = max(-100, min(100, speed_percentage))  # 限制速度范围 0% 到 100%
        duty_cycle = int(abs(speed_percentage) / 100 * 4095)  # PCA9685 使用 12-bit (0-4095)
        pca.set_pwm(pins["pwm"], 0, duty_cycle)
    
    
    
def set_motor_speed(motor_name, speed_percentage):
    """设置电机速度 (0% 到 100%)，负值表示反转"""
    if motor_name not in motor_pins:
        print(f"Invalid motor name: {motor_name}")
        return
    
    pins = motor_pins[motor_name]
    speed_percentage = max(-100, min(100, speed_percentage))  # 限制速度范围 0% 到 100%
    duty_cycle = int(abs(speed_percentage) / 100 * 4095)  # PCA9685 使用 12-bit (0-4095)
    
    if speed_percentage > 0:
        GPIO.output(pins["in1"], GPIO.HIGH)
        GPIO.output(pins["in2"], GPIO.LOW)
    elif speed_percentage < 0:
        GPIO.output(pins["in1"], GPIO.LOW)
        GPIO.output(pins["in2"], GPIO.HIGH)
    else:
        GPIO.output(pins["in1"], GPIO.LOW)
        GPIO.output(pins["in2"], GPIO.LOW)
    
    pca.set_pwm(pins["pwm"], 0, duty_cycle)

def Car_qian(speed=50):
    print('前进')
    set_motor_speed("motor1_zuoqian", speed)
    set_motor_speed("motor2_youqian", speed)
    set_motor_speed("motor3_zuohou", speed)
    set_motor_speed("motor4_youhou", speed)
    
def Car_hou(speed=50): 
    print('后退')
    set_motor_speed("motor1_zuoqian", -speed)
    set_motor_speed("motor2_youqian", -speed)
    set_motor_speed("motor3_zuohou", -speed)
    set_motor_speed("motor4_youhou", -speed)    

# 左转 
def Car_turn_left(speed=50): 
    print('左转')
    set_motor_speed("motor1_zuoqian", -speed)
    set_motor_speed("motor3_zuohou", -speed)
    
    set_motor_speed("motor2_youqian", speed) 
    set_motor_speed("motor4_youhou", speed)  
# 右转
def Car_turn_right(speed=50):  
    print('右转')
    set_motor_speed("motor1_zuoqian", speed)
    set_motor_speed("motor3_zuohou", speed)
    
    set_motor_speed("motor2_youqian", -speed) 
    set_motor_speed("motor4_youhou", -speed)
    
# 水平左移   
def Car_move_left(speed=50):  
    print('水平左移')
    set_motor_speed("motor1_zuoqian", -speed)
    set_motor_speed("motor2_youqian", speed)
    
    set_motor_speed("motor3_zuohou", speed)
    set_motor_speed("motor4_youhou", -speed)
 # 水平右移
def Car_move_right(speed=50):
    print('水平右移')
    set_motor_speed("motor1_zuoqian", speed)
    set_motor_speed("motor2_youqian", -speed)
    
    set_motor_speed("motor3_zuohou", -speed)
    set_motor_speed("motor4_youhou", speed)
    
    
def Car_stop(speed=0):
    #for m in motor_pins.keys():
        #set_motor_speed(m, 0)
    print('停止')
    set_motor_speed("motor1_zuoqian", 0)
    set_motor_speed("motor2_youqian", 0)
    
    set_motor_speed("motor3_zuohou", 0)
    set_motor_speed("motor4_youhou", 0)
  
# 测试代码
'''
if __name__ == "__main__":
    try:
        
        set_servo_angle(Dgree_pin['pitch'], 45)
        set_servo_angle(Dgree_pin['roll'], 45)
        set_servo_angle(Dgree_pin['yaw'], 45)

        #Car_qian(50)
        #Car_hou(50)
        #Car_turn_left(50)
        #Car_turn_right(50)
        #Car_move_right(50)
        #Car_move_left(50)    

        time.sleep(3)
        #set_motor_allspeed(30)
        #time.sleep(3)
        
        Car_stop()
           
    except KeyboardInterrupt:
        print("手动停止")
        
        Car_stop()
        GPIO.cleanup()
        
    finally:
        
        GPIO.cleanup()
'''