main.py

main.py

@@ -0,0 +1,502 @@
+import os
+import math
+import serial.tools.list_ports
+import serial
+import minimalmodbus
+import time
+import numpy as np
+import cv2
+
+
+class monocular_camera:
+    def __init__(self, camera_id=0):
+        self.cap = cv2.VideoCapture(camera_id, cv2.CAP_V4L2)
+        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)  # 设置分辨率宽
+        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)  # 设置分辨率高
+        self.cap.set(cv2.CAP_PROP_FPS, 30)  # 设置帧率
+        self.num = 0
+        if not self.cap.isOpened():
+            print("Cannot open camera")
+            exit()
+    
+    def grab_imgs(self):
+        ret, frame = self.cap.read()
+        if not ret:
+            print("Can't receive frame (stream end?). Exiting ...")
+            return None
+        # 存储图片
+        # savePath = os.path.join("./path", "Camera_{:0>3d}.png".format(self.num))
+        # cv2.imwrite(savePath, frame)
+        # self.num += 1
+        # 显示帧
+        # cv2.imshow("frame", frame)
+        # cv2.waitKey(1000)
+        return frame
+
+
+def capture_point(calib, region_idx):
+
+    print("拍照")
+    picture = cam3.grab_imgs() 
+    # weed
+    img_weed = cv2.GaussianBlur(picture, (5, 5), 0)
+    hsv = cv2.cvtColor(img_weed, cv2.COLOR_BGR2HSV)
+    low_hsv = np.array([33, 43, 46])
+    high_hsv = np.array([99, 255, 255])
+    mask_green = cv2.inRange(hsv, lowerb=low_hsv, upperb=high_hsv)
+    contours, _ = cv2.findContours(mask_green.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+    contours_l = []  
+    weed_list = []
+    center_xy_list = []
+
+
+    for i in range(len(contours)):
+        area = cv2.contourArea(contours[i])
+        if area > 10:
+            contours_l.append(contours[i]) 
+            cnt = contours[i]
+            x_weed, y_weed, w_weed, h_weed = cv2.boundingRect(cnt)
+            center_x, center_y = (x_weed + w_weed / 2), (y_weed + h_weed / 2)
+            weed_list.append([int(center_x), int(center_y)])
+            p_laser_x = (calib[0][0] * center_x) + (calib[0][1] * center_y) + calib[0][2]
+            p_laser_y = (calib[1][0] * center_x) + (calib[1][1] * center_y) + calib[1][2]
+            encoder_position_x = int(p_laser_x) * 91
+            encoder_position_y = int(p_laser_y) * 91
+            if region_idx == 1:
+                if encoder_position_x > -10920 and encoder_position_x < 10920 and encoder_position_y  > - 10920 and encoder_position_y < 10920:
+                    center_xy_list.append([encoder_position_x, encoder_position_y - 455]) # -455是车辆前进的补偿
+            if region_idx == 2:
+                if encoder_position_x > -10920 and encoder_position_x < 10920 and encoder_position_y > - 10920 and encoder_position_y < 10920:
+                    center_xy_list.append([encoder_position_x, encoder_position_y - 455]) # -455是车辆前进的补偿
+            if region_idx == 3:
+                if encoder_position_x > -10920 and encoder_position_x < 10920 and encoder_position_y > - 10920 and encoder_position_y < 10920:
+                    center_xy_list.append([encoder_position_x, encoder_position_y - 455]) # -455是车辆前进的补偿
+            if region_idx == 4:
+                if encoder_position_x > -10920 and encoder_position_x < 10920 and encoder_position_y > - 10920 and encoder_position_y < 10920:
+                    center_xy_list.append([encoder_position_x, encoder_position_y - 455]) # -455是车辆前进的补偿
+        else:
+            continue
+    center_xy_list = sorted(center_xy_list, key=lambda x: x[1], reverse=True) # 按y轴从大到小排序
+    print("center_xy_list", center_xy_list)
+
+    return center_xy_list
+
+def capture_point_data(idx):
+    camera_list = [Red_Control_board_1.A_Dev.camera, Red_Control_board_2.A_Dev.camera, Red_Control_board_3.A_Dev.camera, Red_Control_board_4.A_Dev.camera]
+    calib = camera_list[idx-1].Calibration_Parameter
+    region_idx = camera_list[idx-1].Camera_Region
+    camera_list[idx-1].coordinate_all = capture_point(calib, region_idx)
+    if len(camera_list[0].coordinate_all) > 0:
+        camera_list[0].coordinate_all = [camera_list[0].coordinate_all[0]]
+        camera_list[0].coordinate_all.append([0, 0])
+    camera_list[idx-1].coordinate_all_empty = False
+
+    # camera_list = [Red_Control_board_3.A_Dev.camera]
+    # calib = camera_list[0].Calibration_Parameter
+    # region_idx = camera_list[0].Camera_Region
+    # camera_list[0].coordinate_all = capture_point(calib, region_idx)
+    # if len(camera_list[0].coordinate_all) > 0:
+    #     camera_list[0].coordinate_all = [camera_list[0].coordinate_all[0]]
+    #     camera_list[0].coordinate_all.append([0, 0])
+    # camera_list[0].coordinate_all_empty = False
+
+def convert_to_modbus_format(value):
+    # 如果值为负数，转换为 Modbus 无符号整数形式
+    if value < 0:
+        value += 65536  # 或者 value = value & 0xFFFF，这种方法也可用于确保值在 0 到 65535 之间
+    return value
+
+def is_timeout(start_time, timeout):
+    return time.time() - start_time >= timeout
+
+class camera:
+    def __init__(self,Calibration_Parameter,Camera_Regio):
+        self.Calibration_Parameter  = Calibration_Parameter
+        self.Camera_Region          = Camera_Regio 
+        self.coordinate_all         = [] 
+        self.coordinate_all_empty   = True 
+        self.coordinate_list        = [] 
+        self.num_x                  = - 9100
+        self.num_y                  = - 9100
+
+    def get_coordinate(self):
+        print("现在是区间序号为" + str(self.Camera_Region) + "标定参数为" + str(self.Calibration_Parameter) + "来取值了")
+        self.coordinate_list.clear()
+
+
+        if len(self.coordinate_all) > 0:
+            self.num_x = self.coordinate_all[0][0]
+            self.num_y = self.coordinate_all[0][1]
+            self.coordinate_all.pop(0)
+            self.coordinate_list.append([self.num_x, self.num_y]) 
+            self.coordinate_list = self.coordinate_list[0] # 降维
+        else:
+            # 没有坐标了 设置标志位通知外面拍照 直接结束函数
+            self.coordinate_all_empty = True  # 空
+            self.coordinate_list.clear()
+
+        print("get_coordinate", self.coordinate_list)
+        return self.coordinate_list
+
+
+class Laser :
+    def __init__(self,Modbus_dev,Calibration_Parameter_this,Camera_Regio_this):
+        self.camera = camera(Calibration_Parameter_this,Camera_Regio_this)
+        self.Modbus_dev = Modbus_dev
+        self.start_time = time.time()
+        self.Coordinate_buf = []
+        #寄存器定义
+        self.Laser_100x_voltage_reg     = 33
+        self.Laser_100x_current_reg     = 34
+        self.Laser_time_reg             = 35
+        self.Laser_switch_reg           = 36
+        self.Motor_X_mode_reg           = 37
+        self.Motor_Y_mode_reg           = 38
+        self.Motor_X_acceleration_reg   = 39
+        self.Motor_Y_acceleration_reg   = 40
+        self.Motor_X_speed_reg          = 41
+        self.Motor_Y_speed_reg          = 42
+        self.X_reg                      = 43
+        self.Y_reg                      = 44
+        self.Start_reg                  = 45
+        self.Servo_finish_reg           = 46
+        self.Laser_finish_reg           = 47
+        #状态机定义
+        self.state = 'Init_State'
+        self.transitions = {
+            'Init_State'  : self.Init_State_Func,
+            'IDLE_State'  : self.IDLE_State_Func,
+            'Set_XY_State': self.Set_XY_Func    ,
+            'Start_State' : self.Start_Func     ,
+            'Wait_State'  : self.Wait_State_Func,
+            'Err_State'   : self.Err_State_Func ,
+        }
+
+    def process(self):
+        self.transitions[self.state]()
+
+    def Updata_State(self, new_state):
+        if new_state in self.transitions:
+            self.state = new_state
+        else:
+            print(f"无法转换到未知状态: {new_state}")
+            pass
+
+    def Init_State_Func(self):
+        print("Init_State_Func")
+        Noused_Current_TIMEms_OFF_XM_YM_XA_YA_XV_YV = [0,10,300,0,1,1,2000,2000,2000,2000] # 能量范围[0-100]
+        try:
+            read_finish_register = self.Modbus_dev.read_registers(self.Servo_finish_reg, 1)
+
+        except Exception as e:
+                self.Updata_State('Init_State')
+                print("Wait_State_err")
+        else:
+            #读寄存器错成功 如果初次上电,激光为忙碌,中间断掉再启动激光为空闲,所以这里不判断激光
+            if read_finish_register[0] == 1:
+                #表示上电成功
+                try:
+                    self.Modbus_dev.write_registers(self.Laser_100x_voltage_reg, Noused_Current_TIMEms_OFF_XM_YM_XA_YA_XV_YV)#由于是连续的，所以写一次
+                    self.start_time = time.time()
+                except Exception as e:
+                    if is_timeout(self.start_time, 3):
+                        #写入寄存器错误已经超时
+                        self.Updata_State('Init_State')
+                        print("Init_State_err")
+                    else:
+                        #写入寄存器错误但未超时
+                        self.Updata_State('Init_State')
+                        print("Init_State")
+
+                else:
+                    #写入寄存器错成功
+                    # print("laser power write succeed")
+                    read_power_register = self.Modbus_dev.read_registers(self.Laser_100x_voltage_reg, 10)
+                    read_power_register = list(read_power_register)
+                    print(read_power_register)
+                    if read_power_register == Noused_Current_TIMEms_OFF_XM_YM_XA_YA_XV_YV:
+                        self.Updata_State('IDLE_State')
+                        print("Init_State_over")
+                    else:
+                        self.Updata_State('Init_State')
+                        print("Init_State")
+            else:
+                #表示上电还没成功
+                self.Updata_State('Init_State')
+
+    def Open_State_Func(self):
+        print("Open_State_Func")
+        Noused_Current_TIMEms_OFF_XM_YM_XA_YA_XV_YV = [0,10,300,0,1,1,2000,2000,2000,2000] # 能量范围[0-100]
+        try:
+            read_finish_register = self.Modbus_dev.read_registers(self.Servo_finish_reg, 1)
+
+        except Exception as e:
+                # self.Updata_State('Close_State')
+                print("Close_State_err")
+        else:
+            #读寄存器错成功 如果初次上电,激光为忙碌,中间断掉再启动激光为空闲,所以这里不判断激光
+            if read_finish_register[0] == 1:
+                #表示上电成功
+                try:
+                    self.Modbus_dev.write_registers(self.Laser_100x_voltage_reg, Noused_Current_TIMEms_OFF_XM_YM_XA_YA_XV_YV)#由于是连续的，所以写一次
+                    self.start_time = time.time()
+                except Exception as e:
+                    if is_timeout(self.start_time, 3):
+                        #写入寄存器错误已经超时
+                        # self.Updata_State('Close_State')
+                        print("Close_State_err")
+                    else:
+                        #写入寄存器错误但未超时
+                        # self.Updata_State('Close_State')
+                        print("Close_State")
+
+                else:
+                    #写入寄存器错成功
+                    # print("laser power write succeed")
+                    read_power_register = self.Modbus_dev.read_registers(self.Laser_100x_voltage_reg, 10)
+                    read_power_register = list(read_power_register)
+                    print(read_power_register)
+                    if read_power_register == Noused_Current_TIMEms_OFF_XM_YM_XA_YA_XV_YV:
+                        # self.Updata_State('Mid_State')
+                        print("Init_State_over")
+                    else:
+                        # self.Updata_State('Close_State')
+                        print("Init_State")
+            else:
+                #表示上电还没成功
+                # self.Updata_State('close_State')
+                pass
+
+    def Close_State_Func(self):
+        print("Close_State_Func")
+        Noused_Current_TIMEms_OFF_XM_YM_XA_YA_XV_YV = [0,0,100,0,1,1,2000,2000,2000,2000] # 时间表示在中心带停留的时间
+        try:
+            read_finish_register = self.Modbus_dev.read_registers(self.Servo_finish_reg, 1)
+
+        except Exception as e:
+                # self.Updata_State('Close_State')
+                print("Close_State_err")
+        else:
+            #读寄存器错成功 如果初次上电,激光为忙碌,中间断掉再启动激光为空闲,所以这里不判断激光
+            if read_finish_register[0] == 1:
+                #表示上电成功
+                try:
+                    self.Modbus_dev.write_registers(self.Laser_100x_voltage_reg, Noused_Current_TIMEms_OFF_XM_YM_XA_YA_XV_YV)#由于是连续的，所以写一次
+                    self.start_time = time.time()
+                except Exception as e:
+                    if is_timeout(self.start_time, 3):
+                        #写入寄存器错误已经超时
+                        # self.Updata_State('Close_State')
+                        print("Close_State_err")
+                    else:
+                        #写入寄存器错误但未超时
+                        # self.Updata_State('Close_State')
+                        print("Close_State")
+
+                else:
+                    #写入寄存器错成功
+                    # print("laser power write succeed")
+                    read_power_register = self.Modbus_dev.read_registers(self.Laser_100x_voltage_reg, 10)
+                    read_power_register = list(read_power_register)
+                    print(read_power_register)
+                    if read_power_register == Noused_Current_TIMEms_OFF_XM_YM_XA_YA_XV_YV:
+                        # self.Updata_State('Mid_State')
+                        print("Init_State_over")
+                    else:
+                        # self.Updata_State('Close_State')
+                        print("Init_State")
+            else:
+                #表示上电还没成功
+                # self.Updata_State('close_State')
+                pass
+        
+    def IDLE_State_Func(self):
+        print("IDLE_State_Func",self.Coordinate_buf)
+        if not self.Coordinate_buf:
+            # 表示没有值可以打
+            # 拍照获取值
+            self.Coordinate_buf = [] # 这里为什么要清空一次列表？？
+            self.Coordinate_buf.append(camera.get_coordinate(self.camera))
+            self.Coordinate_buf = self.Coordinate_buf[0]
+            self.Updata_State('IDLE_State')
+            print("get coordinate")
+        else:
+            # 表示列表里还有值 那就去打
+            print("准备去打")
+            if len(self.Coordinate_buf ) == 2:
+                if self.Coordinate_buf[0] == 0 and self.Coordinate_buf[1] == 0:
+                    self.Close_State_Func()
+                else:
+                    self.Open_State_Func()
+                self.start_time = time.time()
+                self.Updata_State('Set_XY_State')
+                print("IDLE_to_Set_XY")
+            else:
+                self.Coordinate_buf.clear()
+                self.Updata_State('IDLE_State')
+
+
+
+    def Set_XY_Func(self):
+        # print("Set_XY")
+        try:
+            converted_values = [convert_to_modbus_format(value) for value in self.Coordinate_buf]
+            self.Modbus_dev.write_registers(self.X_reg, converted_values)#由于是连续的，所以写一次
+        except Exception as e:
+            if is_timeout(self.start_time, 3):
+                #写入寄存器错误已经超时
+                self.Updata_State('Err_State')
+                print("Set_XY_err")
+                self.Coordinate_buf.clear()
+
+            else:
+                #写入寄存器错误但未超时
+                self.Updata_State('Set_XY_State')
+                # print("Set_XY_run")
+        else:
+            #写入寄存器错成功
+            self.start_time = time.time()
+            self.Updata_State('Start_State')
+            print("Set_XY_OK")
+            self.Coordinate_buf.clear()
+
+    def Start_Func(self):
+        print("Start_Func")
+        try:
+            self.Modbus_dev.write_registers(self.Start_reg, [1])
+        except Exception as e:
+            if is_timeout(self.start_time, 3):
+                #写入寄存器错误已经超时
+                self.Updata_State('Err_State')
+                print("Start_State_err")
+            else:
+                #写入寄存器错误但未超时
+                self.Updata_State('Start_State')
+                print("Start_State_run")
+
+        else:
+            #写入寄存器错成功
+            self.start_time = time.time()
+            self.Updata_State('Wait_State')
+            print("Start_State_ok")
+
+    def Wait_State_Func(self):
+        print("Wait_State_Func")
+        try:
+            read_laser_register = self.Modbus_dev.read_registers(self.Servo_finish_reg, 2)
+            print(read_laser_register)
+        except Exception as e:
+            if is_timeout(self.start_time, 3):
+                #读寄存器错误已经超时
+                self.Updata_State('Err_State')
+                print("Wait_State_err")
+
+            else:
+                #读寄存器错误但未超时
+                self.Updata_State('Wait_State')
+                print("Wait_State_run")
+        else:
+            #读寄存器错成功
+            if (read_laser_register[0] == 1 and read_laser_register[1] == 1):
+                #表示激光器完成
+                self.Updata_State('IDLE_State') # 注释
+                print("Wait_State_ok")
+            else:
+                #表示没完成
+                if is_timeout(self.start_time, 90):#电机最长运行30s 激光最长1min
+                    #没完成并且超时
+                    self.Updata_State('Err_State')
+                    print("Wait_StateErr")
+                else:
+                    #没完成但未超时
+                    self.Updata_State('Wait_State')
+                    print("Wait_State_loop")
+
+    
+
+    def Err_State_Func(self):
+        print("Err_State_Func")
+        print("卡死 : 人工检查设备错误")
+        print("准备恢复为初始状态")
+        self.Updata_State('Init_State')
+
+
+
+class Control_board :
+    def __init__(self,COM,Slave_address,Calibration_Parameter,Camera_Regio):
+        self.modbus = minimalmodbus.Instrument(COM, Slave_address)  # 端口名, 从站地址
+        self.modbus.serial.baudrate = 115200  # 波特率
+        self.modbus.serial.bytesize = 8
+        self.modbus.serial.parity = serial.PARITY_NONE
+        self.modbus.serial.stopbits = 1
+        self.modbus.serial.timeout = 1  # seconds
+        self.A_Dev = Laser(self.modbus,Calibration_Parameter,Camera_Regio)
+
+    def process(self):
+        self.A_Dev.process()
+
+
+if __name__ == "__main__":
+
+    transform1 = [[ 9.78925834e-01,  1.96192472e-02, -2.97419867e+02],
+                    [ 3.01586932e-02, -9.63219883e-01,  3.61695282e+02]]
+
+    transform2 = [[ 9.78925834e-01,  1.96192472e-02, -2.97419867e+02],
+                    [ 3.01586932e-02, -9.63219883e-01,  3.61695282e+02]]
+
+    transform3 = [[ 1.55162446e+00, -4.99468032e-03, -5.35657043e+02],
+                    [-3.34157095e-03, -1.54830655e+00,  4.28145094e+02]]
+
+    transform4 = [[ 1.55162446e+00, -4.99468032e-03, -5.35657043e+02],
+                    [-3.34157095e-03, -1.54830655e+00,  4.28145094e+02]]
+    
+    print("开始初始化相机")
+    cam1 = monocular_camera(camera_id=1)
+    cam2 = monocular_camera(camera_id=1)
+    cam3 = monocular_camera(camera_id=0)
+    cam4 = monocular_camera(camera_id=1)
+    cam_list = [cam1, cam2, cam3, cam4]
+    print("相机初始化完毕")
+
+    # 设备注册
+    Red_Control_board_1 = Control_board("COM3",1,transform1,1)
+    Red_Control_board_2 = Control_board("COM3",2,transform2,2)
+    Red_Control_board_3 = Control_board("COM3",3,transform3,3)
+    Red_Control_board_4 = Control_board("COM3",4,transform4,4)
+
+    while True:
+        Red_Control_board_1.process()
+        Red_Control_board_2.process()
+        Red_Control_board_3.process()
+        Red_Control_board_4.process()
+        # 如果4个列表都为空
+        if Red_Control_board_1.A_Dev.camera.coordinate_all_empty :
+            print("1号设备空了")
+            if Red_Control_board_1.A_Dev.state == "IDLE_State" :
+                print("1号设备都空了，且在IDLE_State")
+                capture_point_data(idx=1)
+            else:
+                continue
+        if Red_Control_board_1.A_Dev.camera.coordinate_all_empty :
+            print("2号设备空了")
+            if Red_Control_board_1.A_Dev.state == "IDLE_State" :
+                print("2号设备都空了，且在IDLE_State")
+                capture_point_data(idx=2)
+            else:
+                continue
+        if Red_Control_board_1.A_Dev.camera.coordinate_all_empty :
+            print("3号设备空了")
+            if Red_Control_board_1.A_Dev.state == "IDLE_State" :
+                print("3号设备都空了，且在IDLE_State")
+                capture_point_data(idx=3)
+            else:
+                continue
+        if Red_Control_board_1.A_Dev.camera.coordinate_all_empty :
+            print("4号设备空了")
+            if Red_Control_board_1.A_Dev.state == "IDLE_State" :
+                print("4号设备都空了，且在IDLE_State")
+                capture_point_data(idx=4)
+            else:
+                continue
+        else:
+            continue