; Local storage ; for camera lit CAMERA_IP_ADDR "169.254.168.71" lit CAMERA_PORT 502 ; variable definition FLOAT CameraData[21] ; data read on the camera through Modbus TCP int retcode, mbc int retTrigger int value int CntGoodRead ; succesfull read counter int CntBadRead ; error counter int CntGoodWrite int CntBadWrite INT cameraPhase INT addr_coil = 1010 INT how_many = 5 INT number_reg = 1 INT addr_registers ; function Function HndCamera() Select(cameraPhase)
case -1 ; idle. In this example, for better debug, after each phase the sw come back here. break
case 1 ; Camera Modbus Communication Inizialization mbc = MBC_INIT(CAMERA_IP_ADDR, CAMERA_PORT, 2.0, 2.0, 1) if(mbc <= 0) alarm_set(4001,0,mbc) bCameraInitOk = FALSE cameraPhase = -1 else bCameraInitOk = TRUE cameraPhase = 2 endif break
case 2 ; send the command to do the photo value = 1 retTrigger = MBC_WRITE_COILS(mbc, addr_coil, how_many, value)
select (retTrigger)
case MBC_OK ; 0 we get the answer put the data in the correct variables cameraPhase = -1 INC(CntGoodWrite) break
case MBC_INVAL ; -1 Illegal arguments case MBC_BUSY ; -2 Resource busy case MBC_PERM ; -3 Permesso negato case MBC_CONN ; -4 Bad Connection case MBC_NOBUFS ; -5 Buffer esauriti case MBC_NOMEM ; -6 Memoria insufficiente case MBC_TX_ERR ; -7 Errore di trasmissione case MBC_RX_ERR ; -8 Errore di ricezione case MBC_TIMEOUT ; -9 Timeout comando bloccante case MBC_EXC_CODE_1 ; -10 Risposta con eccezione 1 (function code non supportato dal server) case MBC_EXC_CODE_2 ; -11 Risposta con eccezione 2 (address non valido) case MBC_EXC_CODE_3 ; -12 Risposta con eccezione 3 (quantity o valore non valido) case MBC_EXC_CODE_4 ; -13 Risposta con eccezione 4 (operazione fallita) case MBC_WRONG_ANSWER ; -14 Risposta non valida case MBC_INVAL_MBC ; -15 id MBC non valido default INC(CntBadWrite) ; everytime we read bad data we increment the variable alarm_set(4002, 0, RetCode) bCameraOk = FALSE cameraPhase = -1 break end_select break
case 3 ; Camera Modbus Communication get data RetCode = MBC_READ_INP_REG(mbc, addr_registers, number_reg, EM_MB_32, CameraData) select (RetCode)
case MBC_OK ; 0 we get the answer put the data in the correct variables
; for (i=0,i<21,i++) ; NVRR(_IDX_DATA_CAMERA + i) = cameraData[i] ; end_for
rr1_g_x_pick = CameraData[0] ;bIG green Triangular Xpoint rr1_g_y_pick = CameraData[1] ;bIG green Triangular Ypoint rr1_g_r_pick = CameraData[2] ;bIG green Triangular Rpoint
rr2_o_x_pick = CameraData[3] ;bIG orange Triangular Xpoint rr2_o_y_pick = CameraData[4] ;bIG orange Triangular Ypoint rr2_o_r_pick = CameraData[5] ;bIG orange Triangular Rpoint
rr3_v_x_pick = CameraData[6] ;violet parallelogram Xpoint rr3_v_y_pick = CameraData[7] ;violet parallelogram Ypoint rr3_v_r_pick = CameraData[8] ;violet parallelogram Rpoint
rr4_b_x_pick = CameraData[9] ;small blue Triangular Xpoint rr4_b_y_pick = CameraData[10] ;small blue Triangular Ypoint rr4_b_r_pick = CameraData[11] ;small blue Triangular Rpoint
rr5_y_x_pick = CameraData[12] ;yellow square Xpoint rr5_y_y_pick = CameraData[13] ;yellow square Ypoint rr5_y_r_pick = CameraData[14] ;yellow square Rpoint
rr6_p_x_pick = CameraData[15] ;small pink Triangular Xpoint rr6_p_y_pick = CameraData[16] ;small pink Triangular Ypoint rr6_p_r_pick = CameraData[17] ;small pink Triangular Zpoint
rr7_r_x_pick = CameraData[18] ;small red Triangular Xpoint rr7_r_y_pick = CameraData[19] ;small red Triangular Ypoint rr7_r_r_pick = CameraData[20] ;small red Triangular Zpoint
INC(CntGoodRead) ; for diagnostic everytime we read good data we increment the variable bCameraOk = TRUE ; for diagnostic cameraPhase = -1 break
case MBC_INVAL ; -1 Illegal arguments case MBC_BUSY ; -2 Resource busy case MBC_PERM ; -3 Permesso negato case MBC_CONN ; -4 Bad Connection case MBC_NOBUFS ; -5 Buffer esauriti case MBC_NOMEM ; -6 Memoria insufficiente case MBC_TX_ERR ; -7 Errore di trasmissione case MBC_RX_ERR ; -8 Errore di ricezione case MBC_TIMEOUT ; -9 Timeout comando bloccante case MBC_EXC_CODE_1 ; -10 Risposta con eccezione 1 (function code non supportato dal server) case MBC_EXC_CODE_2 ; -11 Risposta con eccezione 2 (address non valido) case MBC_EXC_CODE_3 ; -12 Risposta con eccezione 3 (quantity o valore non valido) case MBC_EXC_CODE_4 ; -13 Risposta con eccezione 4 (operazione fallita) case MBC_WRONG_ANSWER ; -14 Risposta non valida case MBC_INVAL_MBC ; -15 id MBC non valido default INC(CntBadRead) ; everytime we read bad data we increment the variable alarm_set(4002, 0, RetCode) bCameraOk = FALSE cameraPhase = -1 break end_select break end_select end_fun |
