十九届智能车百度组车模，随手寻个线先。
改良了以前遍历像素慢得扣的问题，用numpy解决了这个问题
#encoding: utf-8
import cv2
import numpy as np
import serial
import time
import sys
from simple_pid import PID
p = PID(Kp=0.23 , Ki=0, Kd=0.01,setpoint=320, output_limits=(-50, 50))
left_start = 0
right_start = 640
up_start = 190
down_start = 250
pressed_pix = 2
speed = 50
def find_value(img11):
# 计算灰度直方图
global left_start, right_start, up_start, down_start
finally_list = []
mid_num = int((right_start + left_start) / 2)
for i in range(left_start, right_start):
# error_now = i - mid_num
n = 0
for j in range(up_start, down_start):
if img11[j, i] >= 128:
n = n + 1
else:
pass
finally_list.append(n)
#print(finally_list)
'''
x = np.array(range(0,len(finally_list)))
y = np.array(finally_list)
plt.bar(x,y,0.8)
plt.show()
'''
left_line = left_start
right_line = right_start
# fina_error = int(fina_error/sigema + mid_num)
for i in range(mid_num, right_start-left_start):
if finally_list[i] >= pressed_pix:
right_line = i + left_start
#print(i)
break
else:
pass
for i in range(mid_num, 0, -1):
if finally_list[i] >= 1+pressed_pix:
left_line = i + left_start
#print(i)
break
else:
pass
line = (left_line + right_line) / 2
return line, left_line, right_line
def find_value_faster(img11):
global left_start, right_start, up_start, down_start
# 提取需要处理的部分图像
img_part = img11[up_start:down_start, left_start:right_start]
# 将图像转换为布尔类型的数组，表示像素值是否大于等于128
bool_img = img_part >= 128
# 沿水平方向对布尔数组进行求和，得到每列像素值大于等于128的数量
finally_list = np.sum(bool_img, axis=0)
pressed_pix = 2 # 请替换为合适的值
# 计算中间数值
mid_num = int((right_start + left_start) / 2)
i = 320
while finally_list[i] <= pressed_pix and 1<i<639:
i = i + 1
right_line = i
i =320
while finally_list[i] <= pressed_pix and 1<i<639:
i = i - 1
left_line = i
# 查找右边界
pressed_pix = 10 # 请替换为合适的值
line = (left_line + right_line) / 2
return line, left_line, right_line
def init_send():
ser = serial.Serial('/dev/arduino', 115200, timeout=1)
return ser
def send_values(ser, servo_value, motor_value):
# 向串口发送舵机值和电机值
time.sleep(0.05)
ser.write(bytes([servo_value, motor_value]))
while ser.in_waiting:
arduino_feedback = ser.readline().decode()
# print("Arduino:", arduino_feedback)
error_last = 0
error_this = 0
if __name__ == "__main__":
kp = 0.4
try:
#ser = init_send()
dev_ca = cv2.VideoCapture(0)
ser = serial.Serial('/dev/arduino', 115200, timeout=1)
while True:
ret, cap = dev_ca.read()
cap = cv2.resize(cap,(640,320))
img = cap
cap = cv2.cvtColor(cap, cv2.COLOR_BGR2GRAY)
ret,finally_got = cv2.threshold(cap,0,255,cv2.THRESH_OTSU)
finally_got = ~finally_got
line, left_line, right_line = find_value_faster(finally_got)
print(line)
err =int( p(line))
print(err)
send_values(ser, 92+err, speed)
cv2.rectangle(finally_got, (left_start, up_start), (right_start, down_start), (255, 0, 255), 3)
cv2.rectangle(finally_got, (60, up_start), (60, down_start), (255, 0, 255), 3)
cv2.rectangle(img, (left_line, up_start), (right_line, down_start), (255, 0, 255), 3)
cv2.line(img,(int(line), int(down_start)), (int(line), int(up_start)),(255,0,255), 3)
#cv2.rectangle(img, (line, up_start), (line, down_start), (255, 255, 255), 3)
except KeyError:
exit()下位机arduino：
#include <Servo.h>
Servo myservo;
String str;
int pos = 92; //舵机角度
int motor_pwm = 100; //电机数值0 ~255
int pos_mid = 92; // 储存舵机中值角度
int motorPin = 5; // 确定motor控制引脚
int servoPin = 9; //确定servo控制引脚
void setup() {
Serial.begin(115200);
myservo.attach(servoPin); // 该舵机由arduino第9脚控制
pinMode(motorPin, OUTPUT); // 将电机引脚设为输出模式
myservo.write(pos_mid); // 舵机复位
analogWrite(motorPin,motor_pwm); // 电机初始转动
delay(1000);
}
void loop() {
if (Serial.available() >= 2) {
int servoValue = Serial.read();
int motorValue = Serial.read();
// 控制舵机
myservo.write(servoValue);
if (servoValue <40) servoValue=40;
else if(servoValue > 140) servoValue=140;
Serial.print("servo: ");
Serial.println(servoValue);
// 控制电机
analogWrite(motorPin, motorValue);
if (motorValue <0) motorValue=0;
else if(motorValue > 255) motorValue=255;
Serial.print("motor: ");
Serial.println(motorValue);
}
}
Comments NOTHING