generated from yuanbiao/python_templates
502 lines
21 KiB
Python
502 lines
21 KiB
Python
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 |