2023电赛E题心得 – ベルベットルーム

写在前面

比赛完蛋了，下次再来。

方案简介

硬件结构图

红色激光前两题题解

首先前两题我们的方案是直接使用开环控制。

直接记录舵机的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的图像筛选，但是到了比赛的地方我们发现实际的光线和实验室大相径庭，结果完全没有办法寻线，只得现场爆改摄像头亮度才能成功寻线，其次就是没有注意到第二组激光的摄像头要跟着云台走，结果导致了彻底的违规，所以很爆炸。

但是电赛还是我最喜欢的比赛

没有智能车那样强制限制主控类型，也没有那么各个队伍之间超级保密，大家真的是很友好，老师也很理解和提供帮助，总之就是下次还来。