写在前面
比赛完蛋了，下次再来。
方案简介
硬件结构图
红色激光前两题题解
首先前两题我们的方案是直接使用开环控制。
直接记录舵机的pwm值，然后连续控制多级走四个点或者中心点即可。
红色激光第三题方案题解：
首先使用获取一张当前的图像，然后裁切得到一张只含有屏幕部分的该图像。
之后使用大津法进行选取，这样就能获取图片上面的黑框部分。
我们获取黑框的四个点之后可以对两个相邻点之间进行连续采样。
之后我们对舵机进行位置增量PID控制，来连续地追踪那一堆点。这样第三题就完成了。
绿色激光追踪题解
opencv首先hsv提取红色与绿色激光点位置，之后根据位置之差进行x,y方向的pid控制追踪，控制给到下位机的esp32，再控制无刷云台追踪实现闭环。
源码品鉴
无刷电机下位机程序
#include <SimpleFOC.h>
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
BLDCMotor motor = BLDCMotor(11);
BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27, 14);
float target_angle = 0;
float change_angle = 2.8;
int i = 0;
Commander command = Commander(Serial);
void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }
void setup() {
Wire.setPins(33,32);
Wire.begin();
sensor.init(&Wire);
motor.linkSensor(&sensor);
driver.voltage_power_supply = 12;
driver.init();
motor.linkDriver(&driver);
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
motor.controller = MotionControlType::angle;
motor.PID_velocity.P = 0.2f;
motor.PID_velocity.I = 20;
motor.PID_velocity.D = 0;
motor.voltage_limit = 6;
motor.LPF_velocity.Tf = 0.01f;
motor.P_angle.P = 20;
motor.velocity_limit = 10;
Serial.begin(115200);
motor.useMonitoring(Serial);
motor.init();
//motor.initFOC(4.58, Direction::CCW);
motor.initFOC(2.55, Direction::CW);
command.add('T', doTarget, "target angle");
_delay(1000);
}
void loop() {
motor.loopFOC();
//motor.move(target_angle - change_angle);
motor.move(change_angle - target_angle);
command.run();
i++;
if(i%5==0) {
Serial.println(sensor.getAngle());
i = 0;
}
}红色激光上位机程序
1. from class_driver.class_driver import motor_driver
2. import time
3. import RPi.GPIO as GPIO
4.
5. import cv2
6. import numpy as np
7.
8.
9. def get_points(img):
10. h = img.shape[0]
11. w = img.shape[1]
12.
13. for i in range(h): # 上
14. if 255 in img[i]:
15. white_index = np.where(img[i] == 255)
16. point_a = [i, white_index[0][0]]
17. break
18.
19. for i in range(w, 0, -1): # 右
20. if 255 in img[:, i - 1]:
21. white_index = np.where(img[:, i - 1] == 255)
22. point_b = [white_index[0][0], i]
23. break
24.
25. for i in range(h, 0, -1): # 下
26. if 255 in img[i - 1]:
27. white_index = np.where(img[i - 1] == 255)
28. point_c = [i, white_index[0][0]]
29. break
30.
31. for i in range(w): # 左
32. if 255 in img[:, i]:
33. white_index = np.where(img[:, i] == 255)
34. print('d:', )
35. point_d = [white_index[0][0], i]
36. break
37. return [point_a, point_b, point_c, point_d]
38.
39.
40.
41. last_x = 0
42. last_y = 0
43.
44. def img_get(img):
45. global last_x,last_y
46. size_min = 0
47. # 滤波二值化
48. # gs_frame = cv2.GaussianBlur(img, (gs, gs), 1)
49. hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
50. # erode_hsv = cv2.erode(hsv, None, iterations=erode)
51. inRange_hsv = cv2.inRange(hsv, np.array([153, 98, 33]), np.array([179, 255, 255]))
52.
53. img = inRange_hsv
54. # 外接计算
55. cnts = cv2.findContours(img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
56. target_list = []
57. if size_min < 1:
58. size_min = 1
59. for c in cnts:
60. if cv2.contourArea(c) < size_min:
61. continue
62. else:
63. target_list.append(c)
64. #print(target_list)
65. for cnt in target_list:
66. x, y, w, h = cv2.boundingRect(cnt)
67. # cv2.rectangle(img0, (x, y), (x + w, y + h), (0, 255, 0), 2)
68. if len(target_list) == 0:
69. x = last_x
70. y = last_y
71. last_x = x
72. last_y = y
73. return x,y
74.
75. def round():
76. left_driver.raw = float(346)
77. left_driver.lll = float(382)
78. time.sleep(0.4)
79. left_driver.raw = float(303)
80. left_driver.lll = float(382)
81. time.sleep(0.4)
82. left_driver.raw = float(303)
83. left_driver.lll = float(325)
84. time.sleep(0.4)
85. left_driver.raw = float(346)
86. left_driver.lll = float(325)
87. time.sleep(0.4)
88.
89. def center():
90. # 回到中点
91. left_driver.raw = float(323) # 右小 左大
92. left_driver.lll = float(348) # 上大 下小
93.
94.
95. def mini_any(img):
96. point_human_1 = (200, 30)
97. point_human_2 = (460, 280)
98.
99. #point_human_1 = (0, 0)
100. #point_human_2 = (565, 460)
101. img = img[point_human_1[1]:point_human_2[1], point_human_1[0]:point_human_2[0]]
102. #print(img)
103. point_x,point_y = img_get(img)
104. return point_y,point_x
105.
106. def any(img):
107. left_driver.raw = float(333) # 右小 左大
108. left_driver.lll = float(339) # 上大 下小
109.
110.
111. #point_human_1 = (180, 30)
112. point_human_1 = (200, 30)
113. point_human_2 = (460, 280)
114.
115. #point_human_1 = (0, 0)
116. #point_human_2 = (565, 460)
117. img = img[point_human_1[1]:point_human_2[1], point_human_1[0]:point_human_2[0]]
118. #print(img)
119. point_x,point_y = img_get(img)
120. print(point_x,point_y)
121. img = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
122.
123. retval, img = cv2.threshold(img,0,255,cv2.THRESH_BINARY+cv2.THRESH_OTSU)
124.
125. #
126.
127. img = cv2.dilate(img, np.uint8(np.ones((3, 3))), 10)
128. img = cv2.bitwise_not(img)
129. ##img = cv2.erode(img, ,None, iterations=3)
130. cv2.imwrite("233.png",img)
131. box = get_points(img)
132. #print(img.shape)
133. print(box)
134. return box, point_x, point_y
135.
136.
137. def moive_point(mv_x,mv_y):
138. left_driver.raw = left_driver.raw-float(mv_x) # 右小 左大
139. left_driver.lll = left_driver.lll-float(mv_y)
140.
141. def list_cocu(x1,y1,x2,y2):
142. step = 20
143. fin_list = []
144. x_now = x1
145. y_now = y1
146. x_step = int(x2 - x1)/step
147. y_step = int(y2 - y1)/step
148. for i in range(0,step,1):
149. x_now = x_now + x_step
150. y_now = y_now + y_step
151. post = [x_now,y_now]
152. fin_list.append(post)
153. return fin_list
154.
155.
156.
157.
158. def control(list_list,delay):
159. kp = 0.01
160. kd = 0.001
161. ki = 0.02
162. kpa = 0.005
163. kda = 0.001
164. kia = 0.003
165. dt_y = 0
166. dt_x = 0
167. for ps in list_list:
168. print(ps)
169. for i in range(1,20):
170. all_error_x = 0
171. all_error_y = 0
172. error_past_x = 0
173. error_past_y = 0
174. ret, img = cap.read()
175. px,py = mini_any(img)
176. error_now_y = (ps[0] - px)
177. error_now_x = (ps[1] - py)
178. dt_y = error_now_y*kp - (-error_past_y +error_now_y)*kd + all_error_y *ki
179. dt_x = error_now_x*kpa - (-error_past_x +error_now_x)*kda + all_error_y *kia
180. error_past_x = error_now_x
181. error_past_y = error_now_y
182. all_error_x += error_now_x
183. all_error_y += error_now_y
184. print("px:"+str(px)+"py:"+str(py))
185.
186. moive_point(dt_x,dt_y)
187. #print(ps[0] - px,ps[1] - py)
188. #print(ps)
189. #print(px,py)
190. time.sleep(delay)
191.
192.
193.
194.
195.
196. if __name__ == '__main__':
197. cap = cv2.VideoCapture(-1)
198.
199. left_driver = motor_driver("/dev/left_roll",115200)
200.
201. left_driver.start()
202.
203. GPIO.setwarnings(False)
204. GPIO.setmode(GPIO.BCM)
205.
206. GPIO.setup(26,GPIO.IN,pull_up_down=GPIO.PUD_DOWN)
207. GPIO.setup(27,GPIO.IN,pull_up_down=GPIO.PUD_DOWN)
208. GPIO.setup(21,GPIO.OUT)
209. buzze = GPIO.PWM(21,1440)
210. buzze.start(0)
211.
212.
213.
214.
215.
216.
217.
218.
219. while True:
220. print(1)
221. if GPIO.input(27):
222. center()
223. print("center")
224. buzze.ChangeDutyCycle(50)
225. time.sleep(1)
226. buzze.ChangeDutyCycle(0)
227. elif GPIO.input(26):
228. round()
229. print("round")
230. buzze.ChangeDutyCycle(50)
231. time.sleep(1)
232. buzze.ChangeDutyCycle(0)
233. else:
234.
235. ret, img = cap.read()
236. kk,px,py=any(img)
237.
238. list_1 = list_cocu(kk[0][0],kk[0][1],kk[1][0],kk[1][1])
239.
240. list_2 = list_cocu(kk[1][0],kk[1][1],kk[2][0],kk[2][1])
241. list_3 = list_cocu(kk[2][0],kk[2][1],kk[3][0],kk[3][1])
242. list_4 = list_cocu(kk[3][0],kk[3][1],kk[0][0],kk[0][1])
243.
244.
245.
246. while not GPIO.input(26) or not GPIO.input(27):
247. control(list_1,0.02)
248. control(list_2,0.02)
249. control(list_3,0.02)
250. control(list_4,0.02)
绿色激光上位机程序
import cv2
from simple_pid import PID
from class_driver.class_driver import motor_driver
import time
import numpy as np
from simple_pid import PID
def getpoints(img, hsvmin, hsv_max, gs, erode):
img = cv2.GaussianBlur(img, (gs, gs), 1)
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
hsv = cv2.erode(hsv, None, iterations=erode)
dst = cv2.inRange(hsv, hsvmin, hsv_max)
cnts = cv2.findContours(dst, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)[-2]
pos = []
for cnt in cnts:
x, y, w, h = cv2.boundingRect(cnt)
cv2.rectangle(img, (x, y), (x + w, y + h), 255, 2)
pos.append([int(x + w / 2), y + h / 2])
return pos
if __name__ == '__main__':
left_driver = motor_driver("/dev/left_roll",115200)
right_driver = motor_driver("/dev/right_roll",115200)
left_driver.start()
right_driver.start()
time.sleep(0.5)
cap = cv2.VideoCapture(-1)
pid_x = PID(0.0001, 0.0, 0.0, 0)
pid_y = PID(0.0001, 0.0, 0.0, 0)
try:
while True:
ret, frame = cap.read()
# red
red_points = getpoints(frame, np.array([144, 40, 138]), np.array([179, 255, 255]), 7, 0)
# green
green_points = getpoints(frame, np.array([46, 90, 179]), np.array([69, 206, 255]), 7, 0)
if len(red_points)==0:
print('NO red')
red_points = red
continue
else:
red = red_points.copy()
if len(green_points)==0:
print('NO green')
continue
else:
print(red_points[0], green_points[0])
error_x = green_points[0][0] - red_points[0][0]
error_y = green_points[0][1] - red_points[0][1]
move_y = pid_y(error_y)
move_x = pid_x(error_x)
if move_x>1:
move_x = 1
elif move_x<-1:
move_x = -1
if move_y>1:
move_y = 1
elif move_y<-1:
move_y = -1
left_driver.target_position = left_driver.target_position - move_y
right_driver.target_position = right_driver.target_position - move_x
print(error_x, error_y, left_driver.target_position, right_driver.target_position, move_x, move_y)
except KeyboardInterrupt:
left_driver.velocity = 0
right_driver.velocity = 0
time.sleep(0.1)
exit()上下位机通讯程序
import serial
import threading
import time
from simple_pid import PID
class motor_driver:
def __init__(self, port, baudrate):
self.motor_angle = 0
self.velocity = 0
self.target_position = 0
self.error = 0
self.serial_port = serial.Serial(
port=port,
baudrate=baudrate,
bytesize=serial.EIGHTBITS,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
)
def start(self):
p = threading.Thread(target=self.serial_read, args=())
q = threading.Thread(target=self.serial_write, args=())
p.start()
q.start()
time.sleep(0.2)
def serial_read(self):
angle_str = ""
while 114514:
try:
if self.serial_port.inWaiting() > 0:
data = self.serial_port.read().decode()
if data == '\n':
self.motor_angle = float(angle_str)
angle_str = ""
else:
angle_str = angle_str + data
except:
self.velocity = 0
def serial_write(self):
while 114514:
data = 'T' + str(self.target_position) + '\n'
self.serial_port.write(data.encode())
time.sleep(0.02)
if __name__ == '__main__':
left_driver = motor_driver("/dev/left_roll",115200)
right_driver = motor_driver("/dev/right_roll",115200)
left_driver.start()
right_driver.start()
time.sleep(0.5)
try:
while True:
left_driver.motor_angle = 0
right_driver.motor_angle = 0
except:
left_driver.stop()
right_driver.stop()
print('stoped')
left_driver.velocity = 0
right_driver.velocity = 0
time.sleep(0.1)
exit()
现场测评为什么炸了
首先是第三题我们选择了HSV的图像筛选，但是到了比赛的地方我们发现实际的光线和实验室大相径庭，结果完全没有办法寻线，只得现场爆改摄像头亮度才能成功寻线，其次就是没有注意到第二组激光的摄像头要跟着云台走，结果导致了彻底的违规，所以很爆炸。
但是电赛还是我最喜欢的比赛
没有智能车那样强制限制主控类型，也没有那么各个队伍之间超级保密，大家真的是很友好，老师也很理解和提供帮助，总之就是下次还来。