树莓派4路电机速度 - MKT-porter

随笔- 1524 文章- 2 评论- 66 阅读- 127万

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

代码说明

PWM 控制:
- 通过 PCA9685 控制 L298N 的 ENA/ENB，调整 PWM 占空比 来控制电机速度。
- 速度范围 0-100%（代码中自动转换为 16-bit）。
电机方向控制:
- 通过树莓派 GPIO 输出 控制 L298N 的 IN1 ~ IN8。
- GPIO.HIGH / GPIO.LOW 设置 前进/后退/停止。
四种运动模式:
- move_forward(speed): 让 4 个电机 前进。
- move_backward(speed): 让 4 个电机 后退。
- turn_left(speed): 让 左侧电机后退，右侧电机前进。
- turn_right(speed): 让 右侧电机后退，左侧电机前进。
- stop(): 停止所有电机。
运行测试:
- 前进 2 秒
- 后退 2 秒
- 左转 2 秒
- 右转 2 秒
- 停止

硬件连接

main.py

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

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

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

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()
'''