翠欧视觉抓取
ROBOT
INCLUDE "GLOBAL"
DIM l_i_last_product AS BOOLEAN
DIM l_b_callcyl,l_b_cylout_done AS BOOLEAN
robot_select(0)
robot_speed(50, 30, 5)
jump_speed(50, 30, 5, 50, 30, 5)
motor_on()'机器人使能
DIM tmp_str AS STRING(100)
DIM l_b_putng_done, l_b_putok_done AS BOOLEAN
DIM l_i_put_ng, l_i_put_ok AS INTEGER
l_i_put_ng = 0
l_i_put_ok = 0
DIM l_robot_step, l_cly_step, l_io_step AS INTEGER
DIM l_init_photo AS INTEGER
DIM l_cam1_str, l_cam2_str AS STRING(500)
DIM l_cam1flog_str, l_cam2flog_str AS STRING(100)
DIM l_f_cam1date, l_f_cam2date AS FLOAT(60)
'相机1
DIM l_cam1_robot_id, l_cam1_id, l_cam1_pick_num AS INTEGER '机器人id
DIM l_cam1_callpick AS BOOLEAN '呼叫机器人取料信号
DIM l_robpick_done AS BOOLEAN '机器人取料完成
DIM l_i_vibra_num AS INTEGER'震动次数
DIM l_cam1_done_num, l_cam2_done_num AS INTEGER'料盘点位拾取次数
'气杆
DEFCONST di_cly_ret 0'气缸原位
DEFCONST di_cly_ext 0'气缸伸出到位
DIM l_cam1_canphoto AS INTEGER
DIM i_correct_step AS INTEGER
DIM l_temp_str AS STRING(100)
DIM l_i_offset AS FLOAT(10)
DIM l_flog_str AS STRING(100)
'临时点位
DIM l_x_pos AS FLOAT
DIM l_y_pos AS FLOAT
DIM l_z_pos AS FLOAT
DIM l_c_pos AS FLOAT
DIM l_b_okdone_step AS INTEGER
DIM l_box_step AS INTEGER
DIM l_b_boxshake_done AS BOOLEAN
DIM l_b_box_shaking AS BOOLEAN
DIM l_i_xiaoliao_num AS INTEGER'多次震动，白盒依然没料的次数
'计时器清零
DIM i AS INTEGER
FOR i = 0 TO 30
delay_clr(i)
NEXT i
'START 初始化
OP(do_vac,OFF)
jumpj(p_save_1)
wait_inpos()
l_cam1_done_num = 0
OP(do_cyl,OFF)
OP(do_vibra, OFF)'震动盘关
l_i_vibra_num = 0
VR(v_d_cam2_sing) = 1
l_b_cylout_done = FALSE
VR(v_d_pal_prodnum) = 0
TRIGGER
'STOP
WHILE 1
IF IN(di_c_calflag) = ON THEN
GOSUB auto_io_control '上料自动流程
GOSUB auto_robot '机械手
GOSUB auto_cly'下料气缸
ELSE
GOSUB sub_calibratiom '视觉矫正
ENDIF
WEND
'start 上下料自动流程
auto_io_control:
SELECT_CASE l_io_step
CASE 00 '回零
l_io_step = 150
CASE 150
PRINT #20, "RbtTrgCam,1"
l_io_step = 200
CASE 200'拍照后***延时，可**改
IF delay(2, 1000) THEN
delay_clr(2)
l_io_step = 300
ENDIF
CASE 300'接收视觉数据
IF KEY #20 THEN
CHANNEL_READ(20, l_cam1_str)
str_getdata(l_cam1_str, ",", l_f_cam1date)
l_io_step = 400
ENDIF
CASE 400
IF delay(33,800) THEN
l_io_step = 500
delay_clr(33)
ENDIF
CASE 500
IF IN(di_xialiao)=OFF THEN
l_io_step = 600
ENDIF
IF IN(di_xialiao)=ON THEN
l_io_step = 650
l_i_vibra_num=0
ENDIF
CASE 600
l_cam1_pick_num = l_f_cam1date(3)
IF l_cam1_pick_num = 0 THEN
l_io_step = 700 '无料
l_i_xiaoliao_num=0
ELSE
l_io_step = 800'有料
l_i_xiaoliao_num=0
ENDIF
CASE 650
GOSUB sub_box_shake '料盒在震动的情况
IF l_b_boxshake_done THEN
l_i_xiaoliao_num=l_i_xiaoliao_num+1
l_io_step = 150
l_b_boxshake_done=FALSE
l_box_step=0
ENDIF
IF l_i_xiaoliao_num>=3 THEN
l_i_xiaoliao_num=0
l_b_boxshake_done=FALSE
l_box_step=0
user_alarm(7,"连续三次完全无料")
l_io_step =722
ENDIF
CASE 700
IF IN(di_xialiao)=OFF THEN
OP(do_vibra, ON)'震动盘开
l_i_vibra_num = l_i_vibra_num + 1
l_io_step = 710
ENDIF
CASE 710'震动盘关
IF delay(3,2500) THEN
delay_clr(3)
OP(do_vibra, OFF)
l_io_step = 711
ENDIF
CASE 711'静置2s
IF delay(21, 2000) THEN
delay_clr(21)
l_io_step = 720
ENDIF
CASE 720
IF l_i_vibra_num >= 3 THEN
l_io_step = 721
ELSE
l_io_step = 150
ENDIF
CASE 721'
user_alarm(2, "震动三次无料报警")
l_io_step = 722
CASE 722'
IF VR(d_b_hmi_btn).bit_reset =1 THEN
l_io_step = 0
l_i_vibra_num = 0
ENDIF
CASE 800 '有料
l_i_vibra_num=0
l_cam1_callpick = TRUE '呼叫机器人取料标志
l_io_step = 900
CASE 900
IF l_robpick_done=TRUE THEN
l_cam1_callpick = FALSE
l_io_step = 910
ENDIF
CASE 910
IF delay(4, 500) THEN
delay_clr(4)
OP(do_vibra, ON)
l_io_step = 1000
ENDIF
CASE 1000'震动延时
IF delay(5, VR(d_user_vibratime)) THEN
delay_clr(5)
OP(do_vibra, OFF)
l_io_step = 1100
ENDIF
CASE 1100
IF delay(6, 1500) THEN
delay_clr(6)
l_io_step = 150
ENDIF
END_CASE
RETURN
'stop
'start 机械手
auto_robot:
SELECT_CASE l_robot_step
CASE 00
IF l_cam1_callpick = TRUE THEN
l_robpick_done=FALSE
l_robot_step = 200
ENDIF
MODBUS TCP
INCLUDE "VR_GLOAB"
DIM heart1,heart2 AS BOOLEAN
heart1=FALSE
heart2=TRUE
WHILE TRUE
'坐标实时监控
'ROBOT世界坐标监控（随当前调用的“工具坐标系”及“用户坐标系”不同会有所差异）
VR(vr_epi_out_cur_pos_x)=WORLD_DPOS AXIS(0)
VR(vr_epi_out_cur_pos_y)=WORLD_DPOS AXIS(1)
VR(vr_epi_out_cur_pos_z)=WORLD_DPOS AXIS(2)
VR(vr_epi_out_cur_pos_c)=WORLD_DPOS AXIS(5)
'ROBOT关节坐标监控
VR(vr_epi_out_cur_pos_j1)=AXIS_DPOS AXIS(0)
VR(vr_epi_out_cur_pos_j2)=AXIS_DPOS AXIS(1)
VR(vr_epi_out_cur_pos_j3)=AXIS_DPOS AXIS(2)
VR(vr_epi_out_cur_pos_j4)=AXIS_DPOS AXIS(3)
'1秒方波心跳检测
'start
IF heart2 THEN
IF delay(1,1000) THEN
VR(vr_epi_out_heart_beat)=1
heart1=TRUE
heart2=FALSE
delay_clr(1)
ENDIF
ENDIF
IF heart1 THEN
IF delay(2,1000)THEN
VR(vr_epi_out_heart_beat)=2
heart1=FALSE
heart2=TRUE
delay_clr(2)
ENDIF
ENDIF
'stop
'远程控制机器人加减速阈值
'start
IF VR(vr_epi_in_flatacc)>VR(flatacc_max) THEN
VR(flatacc_cur)=VR(flatacc_max)
ELSEIF VR(vr_epi_in_flatacc)<=VR(flatacc_min) THEN
VR(flatacc_cur)=VR(flatacc_min)
ELSE
VR(flatacc_cur)=VR(vr_epi_in_flatacc)
ENDIF
IF VR(vr_epi_in_flatdec)>VR(flatdec_max) THEN
VR(flatdec_cur)=VR(flatdec_max)
ELSEIF VR(vr_epi_in_flatdec)<=VR(flatdec_min) THEN
VR(flatdec_cur)=VR(flatdec_min)
ELSE
VR(flatdec_cur)=VR(vr_epi_in_flatdec)
ENDIF
IF VR(vr_epi_in_liftacc)>VR(liftacc_max) THEN
VR(liftacc_cur)=VR(liftacc_max)
ELSEIF VR(vr_epi_in_liftacc)<=VR(liftacc_min) THEN
VR(liftacc_cur)=VR(liftacc_min)
ELSE
VR(liftacc_cur)=VR(vr_epi_in_liftacc)
ENDIF
IF VR(vr_epi_in_liftdec)>VR(liftdec_max) THEN
VR(liftdec_cur)=VR(liftdec_max)
ELSEIF VR(vr_epi_in_liftdec)<=VR(liftdec_min) THEN
VR(liftdec_cur)=VR(liftdec_min)
ELSE
VR(liftdec_cur)=VR(vr_epi_in_liftdec)
ENDIF
IF VR(vr_epi_in_downacc)>VR(downacc_max) THEN
VR(downacc_cur)=VR(downacc_max)
ELSEIF VR(vr_epi_in_downacc)<=VR(downacc_min) THEN
VR(downacc_cur)=VR(downacc_min)
ELSE
VR(downacc_cur)=VR(vr_epi_in_downacc)
ENDIF
IF VR(vr_epi_in_downdec)>VR(downdec_max) THEN
VR(downdec_cur)=VR(downdec_max)
ELSEIF VR(vr_epi_in_downdec)<=VR(downdec_min) THEN
VR(downdec_cur)=VR(downdec_min)
ELSE
VR(downdec_cur)=VR(vr_epi_in_downdec)
ENDIF
'stop
'PLC控制初始化触发
'start
IF tp_rise(0,VR(vr_epi_in_remote_control).initial) THEN
VR(vr_epi_out_p_curcmd)=0
VR(vr_epi_out_posstatus)=0
VR(vr_epi_out_movestep)=0
VR(vr_epi_out_autostep)=0
tp_rise_clr(0)
ENDIF
'stop
WEND
vison
INCLUDE "VR_GLOAB"
robot_select(0)
robot_speed(30,15,15)
jump_speed(30,20,20,30,15,10)
motor_on()'机器人使能
'IO定义
DEFCONST di_pick 202'允许取料
DEFCONST di_put 203'允许放料
DEFCONST do_excute_photo 207'触发拍照
DEFCONST do_clamp 208'夹子
'点位定义
DEFCONST p_pick 0 '取料位
DEFCONST p_photo 1 '拍照位
DEFCONST p_put 2 '放料位
DIM l_auto_step AS INTEGER
DIM l_temp_str AS STRING(50)
DIM l_i_offset AS FLOAT(3)
WHILE TRUE
SELECT_CASE l_auto_step
CASE 00'判断允许取料
IF IN(di_pick)=ON THEN'允许取料
jumpj(p_pick)
wait_inpos()
l_auto_step=20
ELSE'不允许取料,在取料位上方等待
zp(0)
jumpj(p_pick)
wait_inpos()
l_auto_step=10
ENDIF
CASE 10'到取料位
IF IN(di_pick)=ON THEN
jumpj(p_pick)
wait_inpos()
l_auto_step=20
ENDIF
CASE 20'取料
OP(do_clamp,ON)
WA(300)
l_auto_step=50
CASE 50'去拍照位
jumpj(p_photo)
wait_inpos()
l_auto_step=60
CASE 60'触发拍照
OP(do_excute_photo,ON)
WA(50)
OP(do_excute_photo,OFF)
l_auto_step=70
CASE 70'取拍照结果
IF KEY#20 THEN
CHANNEL_READ(20,l_temp_str)
str_getdata(l_temp_str,",",l_i_offset)
l_auto_step=80
ENDIF
CASE 80 '去放料位
IF IN(di_put)=ON THEN'允许放料
offset(l_i_offset(0),l_i_offset(1),0,l_i_offset(2))
jumpj(p_put)
wait_inpos()
l_auto_step=100
ELSE'不允许放料，上方等待
zp(0)
jumpj(p_put)
wait_inpos()
l_auto_step=90
ENDIF
CASE 90
IF IN(di_put)=ON THEN
offset(l_i_offset(0),l_i_offset(1),0,l_i_offset(2))
jumpj(p_put)
wait_inpos()
l_auto_step=100
ENDIF
CASE 100
OP(do_clamp,OFF)'夹子松开放料
WA(300)
l_auto_step=0
END_CASE
WEND
VR_GLOAB
'EIP通讯参数配置程序
'机器人OUTPUT
DIM vr_epi_out_index AS INTEGER=100
DIM vr_epi_out_mot_state AS INTEGER=vr_epi_out_index+0'远程状态，使用BIT位(示教器界面设置参数)
'Bit0:系统报警 Bit1:程序运行 Bit2:系统使能 Bit3:急停状态 Bit4:程序暂停中
'Bit5:程序复位中 Bit6:系统运行中
DIM vr_epi_out_saftyzone AS INTEGER=vr_epi_out_index+1'干涉区域，使用BIT位(示教器界面设置参数)
'Bit0:干涉区域编号0 Bit1:干涉区域编号1 Bit2:干涉区域编号2 Bit3:干涉区域编号3
'Bit4:'干涉区域编号4 Bit5:干涉区域编号5 Bit6:干涉区域编号6 Bit7:干涉区域编号7
DIM vr_epi_out_pendant_mode AS INTEGER=vr_epi_out_index+2'示教器模式(示教器界面设置参数)
DIM vr_epi_out_demand_bt AS INTEGER=vr_epi_out_index+3'按压按钮状态(示教器界面设置参数)
DIM vr_epi_out_program_mode AS INTEGER=vr_epi_out_index+4'程序模式(示教器界面设置参数)
DIM vr_epi_out_user_level AS INTEGER=vr_epi_out_index+5'用户等级(示教器界面设置参数)
DIM vr_epi_out_active_robot AS INTEGER=vr_epi_out_index+6'机器人激活状态(示教器界面设置参数)
DIM vr_epi_out_select_robot AS INTEGER=vr_epi_out_index+7'选择的机器人(示教器界面设置参数)
DIM vr_epi_out_jogmode AS INTEGER=vr_epi_out_index+8'点动模式(示教器界面设置参数)
DIM vr_epi_out_inch_mode AS INTEGER=vr_epi_out_index+9'寸动模式(示教器界面设置参数)
DIM vr_epi_out_inch_dist AS INTEGER=vr_epi_out_index+10'寸动距离(示教器界面设置参数)
DIM vr_epi_out_manual_speed_ratio AS INTEGER=vr_epi_out_index+11'手动速度(示教器界面设置参数)
DIM vr_epi_out_sys_error AS INTEGER=vr_epi_out_index+12'系统错误状态(示教器界面设置参数)
DIM vr_epi_out_sys_errorcode AS INTEGER=vr_epi_out_index+13'系统错误代码(示教器界面设置参数)
DIM vr_epi_out_hand_sys AS INTEGER=vr_epi_out_index+14'当前手系(示教器界面设置参数)
DIM vr_epi_out_receipe_id AS INTEGER=vr_epi_out_index+15'当前配方编号(示教器界面设置参数)
DIM vr_epi_out_motion AS INTEGER=vr_epi_out_index+16'运动状态(示教器界面设置参数)
DIM vr_epi_out_of_id AS INTEGER=vr_epi_out_index+17'用户坐标系ID(示教器界面设置参数)
DIM vr_epi_out_tool_id AS INTEGER=vr_epi_out_index+18'工具坐标系ID(示教器界面设置参数)
DIM vr_epi_out_auto_speed_rate AS INTEGER=vr_epi_out_index+19'全局速度比率(示教器界面设置参数)
DIM vr_epi_out_alarm_info0 AS INTEGER=vr_epi_out_index+20'报警信息0(示教器界面设置参数)
DIM vr_epi_out_alarm_info1 AS INTEGER=vr_epi_out_index+21'报警信息1(示教器界面设置参数)
DIM vr_epi_out_servo_errcode AS INTEGER=vr_epi_out_index+22'伺服报警代码(示教器界面设置参数)
DIM vr_epi_out_cur_pos_x AS INTEGER=vr_epi_out_index+23'当前机器人世界坐标X位置(机器人程序编程)
DIM vr_epi_out_cur_pos_y AS INTEGER=vr_epi_out_index+24'当前机器人世界坐标Y位置(机器人程序编程)
DIM vr_epi_out_cur_pos_z AS INTEGER=vr_epi_out_index+25'当前机器人世界坐标Z位置(机器人程序编程)
DIM vr_epi_out_cur_pos_c AS INTEGER=vr_epi_out_index+26'当前机器人世界坐标C位置(机器人程序编程)
DIM vr_epi_out_cur_pos_j1 AS INTEGER=vr_epi_out_index+27'当前机器人关节坐标J1位置(机器人程序编程)
DIM vr_epi_out_cur_pos_j2 AS INTEGER=vr_epi_out_index+28'当前机器人关节坐标J2位置(机器人程序编程)
DIM vr_epi_out_cur_pos_j3 AS INTEGER=vr_epi_out_index+29'当前机器人关节坐标J3位置(机器人程序编程)
DIM vr_epi_out_cur_pos_j4 AS INTEGER=vr_epi_out_index+30'当前机器人关节坐标J4位置(机器人程序编程)
DIM vr_epi_out_posstatus AS INTEGER=vr_epi_out_index+31'机器人反馈定位位置状态字(机器人程序编程—
'ROBOT0)
DIM vr_epi_out_heart_beat AS INTEGER=vr_epi_out_index+32'心跳测试(机器人程序编程)(暂未设置)
DIM vr_epi_out_p_curcmd AS INTEGER=vr_epi_out_index+33'当前运动命令符(机器人程序编程—ROBOT0)
DIM vr_epi_out_autostep AS INTEGER=vr_epi_out_index+34'当前自动步序(机器人程序编程—ROBOT0)
DIM vr_epi_out_movestep AS INTEGER=vr_epi_out_index+35'当前运动步序(机器人程序编程—ROBOT0)
'用户报警地址0,连续4个VR(示教器界面设置参数最多存储64个用户报警)
DIM vr_epi_out_useralarm0 AS INTEGER=vr_epi_out_index+36
DIM vr_epi_out_useralarm1 AS INTEGER=vr_epi_out_index+37'用户报警地址1
DIM vr_epi_out_useralarm2 AS INTEGER=vr_epi_out_index+38'用户报警地址2
DIM vr_epi_out_useralarm3 AS INTEGER=vr_epi_out_index+39'用户报警地址3
'输出预留
'DIM vr_epi_out_9 AS INTEGER=vr_epi_out_index+40'预留
'DIM vr_epi_out_8 AS INTEGER=vr_epi_out_index+41'预留
'DIM vr_epi_out_7 AS INTEGER=vr_epi_out_index+42'预留
'DIM vr_epi_out_6 AS INTEGER=vr_epi_out_index+43'预留
'DIM vr_epi_out_5 AS INTEGER=vr_epi_out_index+44'预留
'DIM vr_epi_out_4 AS INTEGER=vr_epi_out_index+45'预留
'DIM vr_epi_out_3 AS INTEGER=vr_epi_out_index+46'预留
'DIM vr_epi_out_2 AS INTEGER=vr_epi_out_index+47'预留
'DIM vr_epi_out_1 AS INTEGER=vr_epi_out_index+48'预留
DIM vr_epi_out_lastend AS INTEGER=vr_epi_out_index+49'预留
'stop
'机器人INPUT
DIM vr_epi_in_index AS INTEGER=150
DIM vr_epi_in_remote_control AS INTEGER=vr_epi_in_index+0'接收PLC发送的远程控制字(示教器界面设置参数)
'Bit0:错误复位 Bit1:程序启动 Bit2:程序停止 Bit3:程序暂停 Bit4:程序使能
'Bit5:程序复位(预留，暂未使用)
DIM initial AS INTEGER=6'(用户PLC程序编程)
DIM vr_epi_in_p_cmd AS INTEGER=vr_epi_in_index+1'接收PLC发送的位置命令符(用户PLC程序编程)
DIM vr_epi_in_manual_speed_ratio AS INTEGER=vr_epi_in_index+2'手动速度(用户PLC程序编程)(暂未使用)
DIM vr_epi_in_global_seed_rate AS INTEGER=vr_epi_in_index+3'全局速度(用户PLC程序编程)
DIM vr_epi_in_x_offset AS INTEGER=vr_epi_in_index+4'接收PLC发送的X方向偏移位置(用户PLC程序编程)
DIM vr_epi_in_y_offset AS INTEGER=vr_epi_in_index+5'接收PLC发送的Y方向偏移位置(用户PLC程序编程)
DIM vr_epi_in_c_offset AS INTEGER=vr_epi_in_index+6'接收PLC发送的旋转C方向偏移位置(用户PLC程序编程)
DIM vr_epi_in_z1 AS INTEGER=vr_epi_in_index+7'接收PLC发送的Z轴位置1(用户PLC程序编程)
DIM vr_epi_in_z2 AS INTEGER=vr_epi_in_index+8'接收PLC发送的Z轴位置2(用户PLC程序编程)
DIM vr_epi_in_z3 AS INTEGER=vr_epi_in_index+9'接收PLC发送的Z轴位置3(用户PLC程序编程)
DIM vr_epi_in_flatacc AS INTEGER=vr_epi_in_index+10'接收PLC发送的平移运动加速度(用户PLC程序编程)
DIM vr_epi_in_flatdec AS INTEGER=vr_epi_in_index+11'接收PLC发送的平移运动减速度(用户PLC程序编程)
DIM vr_epi_in_liftacc AS INTEGER=vr_epi_in_index+12'接收PLC发送的上升运动加速度(用户PLC程序编程)
DIM vr_epi_in_liftdec AS INTEGER=vr_epi_in_index+13'接收PLC发送的上升运动减速度(用户PLC程序编程)
DIM vr_epi_in_downacc AS INTEGER=vr_epi_in_index+14'接收PLC发送的上升运动加速度(用户PLC程序编程)
DIM vr_epi_in_downdec AS INTEGER=vr_epi_in_index+15'接收PLC发送的上升运动减速度(用户PLC程序编程)
'输入预留
'DIM vr_epi_in_13 AS INTEGER=vr_epi_in_index+16'预留
'DIM vr_epi_in_12 AS INTEGER=vr_epi_in_index+17'预留
'DIM vr_epi_in_11 AS INTEGER=vr_epi_in_index+18'预留
'DIM vr_epi_in_10 AS INTEGER=vr_epi_in_index+19'预留
'DIM vr_epi_in_9 AS INTEGER=vr_epi_in_index+20'预留
'DIM vr_epi_in_8 AS INTEGER=vr_epi_in_index+21'预留
'DIM vr_epi_in_7 AS INTEGER=vr_epi_in_index+22'预留
'DIM vr_epi_in_6 AS INTEGER=vr_epi_in_index+23'预留
'DIM vr_epi_in_5 AS INTEGER=vr_epi_in_index+24'预留
'DIM vr_epi_in_4 AS INTEGER=vr_epi_in_index+25'预留
'DIM vr_epi_in_3 AS INTEGER=vr_epi_in_index+26'预留
'DIM vr_epi_in_2 AS INTEGER=vr_epi_in_index+27'预留
'DIM vr_epi_in_1 AS INTEGER=vr_epi_in_index+28'预留
'DIM vr_epi_in_0 AS INTEGER=vr_epi_in_index+29'预留
'stop
'机器人加减速上下阈值参数
DIM velocity_index AS INTEGER=200
DIM flatacc_cur AS INTEGER=velocity_index+0
DIM flatdec_cur AS INTEGER=velocity_index+1
DIM liftacc_cur AS INTEGER=velocity_index+2
DIM liftdec_cur AS INTEGER=velocity_index+3
DIM downacc_cur AS INTEGER=velocity_index+4
DIM downdec_cur AS INTEGER=velocity_index+5
DIM flatacc_max AS INTEGER=velocity_index+10
DIM flatacc_min AS INTEGER=velocity_index+11
DIM flatdec_max AS INTEGER=velocity_index+12
DIM flatdec_min AS INTEGER=velocity_index+13
DIM liftacc_max AS INTEGER=velocity_index+14
DIM liftacc_min AS INTEGER=velocity_index+15
DIM liftdec_max AS INTEGER=velocity_index+16
DIM liftdec_min AS INTEGER=velocity_index+17
DIM downacc_max AS INTEGER=velocity_index+18
DIM downacc_min AS INTEGER=velocity_index+19
DIM downdec_max AS INTEGER=velocity_index+20
DIM downdec_min AS INTEGER=velocity_index+21
FB_GENERAL
'start 程序操作函数
'description:运行新的任务
'function para1:filename,文件名称
'function para2:num,分配的任务号
'function para3:cmd,命令 on/off
FUNCTION fb_proc_ope(filename AS STRING, num AS INTEGER, cmd AS INTEGER) AS BOOLEAN
DIM file_status AS BOOLEAN
file_status = IS_PROG filename
IF cmd = ON THEN
IF (file_status <> 0) AND num > -1 AND num <= 22 THEN
IF PROC_STATUS PROC(num) = 0 THEN
RUN filename, num
ELSE
RETURN FALSE
ENDIF
RETURN TRUE
ELSE
RETURN FALSE
ENDIF
ELSE
file_status = IS_PROG filename
IF file_status THEN
STOP filename
RETURN TRUE
ENDIF
ENDIF
RETURN FALSE
ENDFUNC
'stop
FUNCTION fb_get_flog_pos(l_serch_string AS STRING, l_serch_symbol AS STRING, l_flog AS STRING)
DIM l_len_string AS INTEGER
DIM i AS INTEGER
DIM l_symbol_pos AS INTEGER
l_len_string = LEN(l_serch_string)
FOR i = 0 TO l_len_string - 1
IF l_serch_symbol = MID(l_serch_string, i, 1) THEN
l_symbol_pos = i
i = l_len_string - 1
ENDIF
NEXT i
l_flog = MID(l_serch_string, 0, l_symbol_pos)
ENDFUNC
'start TCP/IP数据解析函数
'description:返回第一个逗号前的字符串
'function para1: str_rec,待解析字符
'function para2: str_mid,间隔符
FUNCTION fb_first_str(str_rec, str_mid AS STRING) AS STRING
'查找间隔符位置
DIM i AS INTEGER
DIM l_i_midpos AS INTEGER(100)
DIM l_i_midnum AS INTEGER
l_i_midnum = 0
FOR i = 0 TO LEN(str_rec) - 1
IF MID(str_rec, i, 1) = str_mid THEN
l_i_midpos(l_i_midnum) = i
l_i_midnum = l_i_midnum + 1
ENDIF
NEXT i
'解析数据
DIM rec_str AS STRING(10)
rec_str = MID(str_rec, 0, l_i_midpos(0))
RETURN rec_str
ENDFUNC
'stop
'start IO沿检测
'Module : fb_io_trig
'Purpose: IO Edge Check
'Created: Aug-24-2022
'Author : Jie Hu
'History:
'instruction
'channel_type 0/1 IN/OP
'edge_type 0/1 rise/fall
FUNCTION fb_io_trig(channel_type,edge_type,io_num AS INTEGER) AS BOOLEAN
STATIC io_pre_state AS INTEGER(2,2,1024)
STATIC io_result AS BOOLEAN(2,2,1024)
STATIC io_trig_step AS INTEGER(2,2,1024)
io_result(channel_type,edge_type,io_num) = FALSE
IF channel_type <> 0 AND channel_type <> 1 OR edge_type <> 0 AND edge_type <> 1 OR io_num > 1023 OR io_num < 0 THEN
RETURN FALSE
ENDIF
SELECT_CASE io_trig_step(channel_type,edge_type,io_num)
CASE 0
IF channel_type = 0 AND edge_type = 0 THEN
io_pre_state(channel_type,edge_type,io_num) = IN(io_num)
io_trig_step(channel_type,edge_type,io_num) = 1
ELSEIF channel_type = 0 AND edge_type = 1 THEN
io_pre_state(channel_type,edge_type,io_num) = IN(io_num)
io_trig_step(channel_type,edge_type,io_num) = 2
ELSEIF channel_type = 1 AND edge_type = 0 THEN
io_pre_state(channel_type,edge_type,io_num) = READ_OP(io_num)
io_trig_step(channel_type,edge_type,io_num) = 3
ELSEIF channel_type = 1 AND edge_type = 1 THEN
io_pre_state(channel_type,edge_type,io_num) = READ_OP(io_num)
io_trig_step(channel_type,edge_type,io_num) = 4
ENDIF
CASE 1'in channel rise edge
IF IN(io_num) = ON AND io_pre_state(channel_type,edge_type,io_num) = 0 THEN
io_result(channel_type,edge_type,io_num) = TRUE
io_pre_state(channel_type,edge_type,io_num) = 1
ENDIF
IF IN(io_num) = OFF AND io_pre_state(channel_type,edge_type,io_num) = 1 THEN
io_result(channel_type,edge_type,io_num) = FALSE
io_pre_state(channel_type,edge_type,io_num) = 0
ENDIF
CASE 2'in channel fall edge
IF IN(io_num) = OFF AND io_pre_state(channel_type,edge_type,io_num) = 1 THEN
io_result(channel_type,edge_type,io_num) = TRUE
io_pre_state(channel_type,edge_type,io_num) = 0
ENDIF
IF IN(io_num) = ON AND io_pre_state(channel_type,edge_type,io_num) = 0 THEN
io_result(channel_type,edge_type,io_num) = FALSE
io_pre_state(channel_type,edge_type,io_num) = 1
ENDIF
CASE 3'op channel rise edge
IF READ_OP(io_num) = ON AND io_pre_state(channel_type,edge_type,io_num) = 0 THEN
io_result(channel_type,edge_type,io_num) = TRUE
io_pre_state(channel_type,edge_type,io_num) = 1
ENDIF
IF READ_OP(io_num) = OFF AND io_pre_state(channel_type,edge_type,io_num) = 1 THEN
io_result(channel_type,edge_type,io_num) = FALSE
io_pre_state(channel_type,edge_type,io_num) = 0
ENDIF
CASE 4 'op channel fall edge
IF READ_OP(io_num) = OFF AND io_pre_state(channel_type,edge_type,io_num) = 1 THEN
io_result(channel_type,edge_type,io_num) = TRUE
io_pre_state(channel_type,edge_type,io_num) = 0
ENDIF
IF IN(io_num) = ON AND io_pre_state(channel_type,edge_type,io_num) = 0 THEN
io_result(channel_type,edge_type,io_num) = FALSE
io_pre_state(channel_type,edge_type,io_num) = 1
ENDIF
END_CASE
RETURN io_result(channel_type,edge_type,io_num)
ENDFUNC
'stop
'start 检测高电平
'Module : fb_smooth_check
'Purpose: Check some high level signal
'Created: Aug-24-2022
'Author : Jie Hu
'History:
'instruction:
'vr_edge_type 0/1 rise/fall
'vr_num vr index
'vr_bit bit of vr
FUNCTION fb_smooth_check(i AS INTEGER,level AS BOOLEAN, smoothtime AS INTEGER) AS BOOLEAN
STATIC l_i_smoothcheck_ticks AS INTEGER(1024)
STATIC l_b_smoothdone AS BOOLEAN(1024)
STATIC l_i_smoothcheck_step AS INTEGER(1024)
SELECT_CASE l_i_smoothcheck_step(i)
CASE 00
l_b_smoothdone(i) = FALSE
IF level = TRUE THEN
l_i_smoothcheck_ticks(i) = TICKS
l_i_smoothcheck_step(i) = 10
ENDIF
CASE 10
IF level = TRUE THEN
IF ABS(TICKS - l_i_smoothcheck_ticks(i)) * SERVO_PERIOD / 1000 > smoothtime THEN
l_b_smoothdone(i) = TRUE
l_i_smoothcheck_step(i) = 20
ENDIF
ELSE
l_i_smoothcheck_step(i) = 00
ENDIF
CASE 20
IF level = OFF THEN
l_i_smoothcheck_step(i) = 0
ENDIF
END_CASE
RETURN l_b_smoothdone(i)
ENDFUNC
'stop
'start 时钟脉冲
'fb_blink:以S为单位控制反馈高低电平，三色灯闪烁输出
'参数：
'1：id:重复使用区别号（0-1023），相同进程内调用,id号不可重复
'2:on_time(整形),ms；
'3:off_time(整形),ms；
'返回值：
'TRUE;FLASE
'使用实例：假设黄色灯输出为10，报警时候需要每5S内亮2S，灭3S，则
'op(10,fb_blink(0,2000,3000)
FUNCTION fb_blink(id, on_time, off_time AS INTEGER) AS INTEGER
STATIC l_b_ret AS INTEGER(100)
STATIC l_i_pre_ini_time,l_i_time_length AS INTEGER(100)
STATIC l_i_blink_step AS INTEGER(100)
SELECT_CASE l_i_blink_step(id)
CASE 00
l_i_pre_ini_time(id) = TICKS
l_i_blink_step(id) = 10
CASE 10
l_i_time_length(id) = ABS(TICKS - l_i_pre_ini_time(id))*SERVO_PERIOD/1000
IF l_i_time_length(id) <= on_time THEN
l_b_ret(id) = ON
ENDIF
IF l_i_time_length(id) > on_time AND l_i_time_length(id) <= on_time + off_time THEN
l_b_ret(id) = OFF
ENDIF
IF l_i_time_length(id) > on_time + off_time THEN
l_i_blink_step(id) = 0
ENDIF
END_CASE
RETURN l_b_ret(id)
ENDFUNC
'stop
SYS
GLOBAL
'机器人自动流程
'VR
DEFCONST d_b_hmi_btn 10000
DEFCONST bit_reset 0 'hmi复位报警
DEFCONST bit_start 1
DEFCONST bit_stop 2
DEFCONST bit_halt 3 '暂停
DEFCONST d_v_tcp_dataflag 11'通道数据标志
DEFCONST v_d_cam2_sing 100'相机2标定点位
DEFCONST v_d_pal_prodnum 101'托盘产品数量
DEFCONST d_v_rob_sts 500
'--===========点位定义================
DEFCONST p_save_1 1 '安全位
DEFCONST p_base_pick 2 '取料基准位
DEFCONST p_cam2_photo 3 '拍照位
DEFCONST p_base_put 4 '放料基准下
DEFCONST p_ng_1 6 ' 1号相机NG位
DEFCONST p_ng_2 7 '2号相机NG位
DEFCONST p_base_cam1_z 10'机器人料盘抓取点位，需要校准j3（z）轴的位置。
DEFCONST p_base_cam2_z 11 '机器人料盘抓取点位，需要校准j3（z）轴的位置。
DEFCONST p_cam1_write 12 '上相机写入点
DEFCONST p_cam2_write 13 '下相机写入点
DEFCONST p_cam1_0 20 '上相机0号点位取料点
'--===========变量定义================
'START
'INVALID TOKEN(S) ON LINE >>> speed_set = 5 - -速度百分比0 - -100%
DEFCONST d_act_put_count 4 '放料个数
DEFCONST d_delay_pick 100' 取料真空延时
DEFCONST d_delay_put 100 '放料真空延时
DEFCONST d_delay_break 200 '破真空时间
DEFCONST d_delay_ms 200'上升沿延时
DEFCONST d_delay_go 200'吸取产品后等待延时离开
DEFCONST d_delay_work 4000 '振动盘动作完成延时拍照时间
DEFCONST d_alram_time 4000 '真空检测处理时间
DEFCONST d_vision_time 5000'检测视觉拍照是否超时
'START 初始化变量
DIM i_pick_num AS INTEGER '接收到视觉取料坐标个数
DIM i_put_num AS INTEGER'接收到视觉放料坐标个数
DIM i_point_id AS INTEGER '标定次数
DIM i_put_cut AS INTEGER '放料计数
DIM i_pick_cam_flag AS INTEGER '上相机拍照完成时置1
DIM i_put_cam_flag AS INTEGER '下相机拍照完成时置1
DIM i_clb_cam_flag AS INTEGER '标定照完成时置1
DIM i_cyl_work_ok AS INTEGER '无杆气缸动作完成
DIM i_work_flag AS INTEGER '是否触发拍照
'-- delay1_val=0
'-- delay1_value=0
'STOP
'STOP
'--============IO定义====================
'START
'--DI
'DEFCONST di_cly_a 121 '气缸原点
'DEFCONST di_cly_b 122 '气缸动点
'DEFCONST di_do_check 123 '真空检测
DEFCONST di_pick_done 130 '机械手取料完成
DEFCONST di_mode 120 '自动 / 标定模式切换
DEFCONST di_pick 141 '校正允许取料
DEFCONST di_re_bait 145 '下料重启
DEFCONST di_xialiao 5 '下料盘下料
DEFCONST di_btn_start 10 '启动按钮
DEFCONST di_btn_stop 6 '暂停按钮
DEFCONST di_btn_res 8 '复位按钮
DEFCONST di_cyl_ret 0
DEFCONST di_cyl_ext 1
DEFCONST di_vac 2
DEFCONST di_c_calflag 4
'--DO
DEFCONST do_vibra 8 '振动盘do
DEFCONST do_vibrating 131 '料仓送料do
DEFCONST do_put_done 132 '下游机械手允许取料信号
DEFCONST do_redlight 4
DEFCONST do_yellowlight 5
DEFCONST do_greenlight 6
DEFCONST do_bee 7
DEFCONST do_cyl 1
DEFCONST do_vac 2
'stop
'--============校准====================
'START
DIM s_cam_pos_flag AS STRING(100)
DIM i_cam_id AS INTEGER
DIM f_x_pos, f_y_pos, f_z_pos, f_c_pos AS FLOAT
DIM f_clbx, f_clby, f_clbz, f_clbc AS FLOAT
'VR
DEFCONST d_v_alarm_sts 20'报警状态
DEFCONST d_v_correct_flage 201 '视觉数据——指令辨别
DEFCONST d_v_correct_cam 202 '视觉数据——相机号
DEFCONST d_v_correct_point 203 '视觉数据——点位
DEFCONST d_v_correct_x 204 '视觉数据——X
DEFCONST d_v_correct_y 205 '视觉数据——Y
DEFCONST d_v_correct_z 206 '视觉数据——Z
DEFCONST d_v_correct_c 209 '视觉数据——C(j4旋转)
'stop
'延时
DIM i_tem_time AS INTEGER
DIM f_vib_time AS FLOAT
DEFCONST d_vib_delay_time 2 '下料延时
'STOP
'--============用户自定义====================
'start
DEFCONST d_user_vibratime 250 '震动延时
DEFCONST d_photo_delaytime 251'拍照延时
DEFCONST v_bait_delaytime 252'下料时间
'stop
'--============真空====================
'状态控制
'start
DEFCONST d_vac_state_close 0 '关闭真空
DEFCONST d_vac_state_open 1'吸真空
DEFCONST d_vac_state_break 2'破真空
DEFCONST d_vac_num 0'真空id
'stop
'--============气缸====================
'start
DEFCONST i_cly1_num 0'气缸号
'stop
'--============自动====================
'START
'相机1
DEFCONST d_v_cam1_flag 300'相机标志位
DEFCONST d_v_cam1_robot 301'机器人id
DEFCONST d_v_cam1_camid 302'相机id
DEFCONST d_v_cam1_num 303'可取个数
'相机2
DEFCONST d_v_cam2_flage 211 '视觉数据——指令辨别
DEFCONST d_v_cam2_robot 212 '视觉数据——机器人
DEFCONST d_d_v_cam2_cam 213 '视觉数据——相机
DEFCONST d_v_cam2_point 214 '视觉数据——点位
DEFCONST d_v_cam2_isvalid 215 '视觉数据——isValid
DEFCONST d_v_cam2_x 216 '视觉数据——x
DEFCONST d_v_cam2_y 217 '视觉数据——y
DEFCONST d_v_cam2_z 218 '视觉数据——z
DEFCONST d_v_cam2_c 219 '视觉数据——C(j4旋转)
DEFCONST d_d_v_key 220 '相机一返回是否有数据
'--============全局IO ====================
SYS_MAIN
INCLUDE "GLOBAL"
DIM l_b_temp,l_b_yellow_flag AS BOOLEAN
DIM l_b_fallflag AS BOOLEAN(3)
DIM l_b_pretemp AS BOOLEAN
WHILE 1
l_b_temp=(tp_get_current_log_line()<>0)
IF l_b_temp THEN
OP(do_redlight,fb_blink(0,500,500))
OP(do_bee,fb_blink(0,500,500))'蜂鸣器
ENDIF
IF VR(d_b_hmi_btn).bit_reset=1 THEN
VR(d_v_alarm_sts)=0
ENDIF
IF VR(d_b_hmi_btn).bit_halt=1 THEN
l_b_yellow_flag = TRUE
ENDIF
IF VR(d_b_hmi_btn).bit_start = 1 THEN
l_b_yellow_flag = FALSE
ENDIF
OP(do_yellowlight, ABS(l_b_yellow_flag))
IF READ_OP(do_redlight) = OFF AND READ_OP(do_yellowlight) = OFF AND PROC_STATUS PROC(19) = 1 THEN
OP(do_greenlight, ON)
ELSE
OP(do_greenlight, OFF)
ENDIF
IF PROC_STATUS PROC(19) = 0 AND NOT l_b_temp AND l_b_fallflag(0)=FALSE THEN
OP(do_greenlight, OFF)
OP(do_yellowlight,OFF)
OP(do_redlight,OFF)
l_b_fallflag(0)=TRUE
ENDIF
IF fb_io_trig(0,0,di_btn_start) THEN'启动
VR(d_b_hmi_btn).bit_start = 1
ENDIF
IF fb_io_trig(0,1,di_btn_start) THEN
VR(d_b_hmi_btn).bit_start = 0
ENDIF
IF fb_io_trig(0,0,di_btn_stop) THEN'暂停
VR(d_b_hmi_btn).bit_halt = 1
ENDIF
IF fb_io_trig(0,1,di_btn_stop) THEN
VR(d_b_hmi_btn).bit_halt = 0
ENDIF
IF fb_io_trig(0,0,di_btn_res) THEN'复位
VR(d_b_hmi_btn).bit_stop = 1
OP(do_bee, OFF) '蜂鸣器
OP(di_vac,OFF)
remote_reset_err(0)
ENDIF
IF fb_io_trig(0,1,di_btn_res) THEN
VR(d_b_hmi_btn).bit_stop = 0
ENDIF
IF fb_smooth_check(0,READ_OP(do_cyl)=ON AND IN(di_cyl_ext)=OFF,10000) THEN
user_alarm(3,"下料气缸到位报警")
VR(10).0=1
ENDIF
IF fb_smooth_check(1,READ_OP(do_cyl)=OFF AND IN(di_cyl_ret)=OFF,10000) THEN
user_alarm(4,"下料气缸原位报警")
VR(10).1=1
ENDIF
' IF fb_smooth_check(2,READ_OP(do_vac)=ON AND IN(di_vac)=OFF,500) THEN
' user_alarm(5,"吸真空报警")
' VR(10).2=1
'ENDIF
IF fb_smooth_check(3,READ_OP(do_vac)=OFF AND IN(di_vac)=ON,500) THEN
user_alarm(6,"没触发真空但是有真空信号")
VR(10).3=1
ENDIF
IF l_b_temp=TRUE THEN
VR(d_b_hmi_btn).bit_stop = 1
ENDIF
WEND
程序1haier
'CO_WRITE_AXIS(8,3226,0,6,-1,
robot_select(0)
robot_speed(50,50,50)
jump_speed(50,20,20,50,20,20)
DIM b0,b1,b2,d0,d1,p_write_x,p_write_y,p_write_z,p_write_c AS INTEGER
DIM str_sec AS STRING(100)
DIM str_rec AS STRING(100)
DIM l_str AS STRING(100,100)
DIM d AS FLOAT(100)
DIM tcp_port AS INTEGER=20
DIM i AS INTEGER
DIM outdata AS FLOAT(500)
DIM t AS FLOAT
DIM x,y,z,c,result AS FLOAT
DIM read AS STRING(100)
DIM write AS STRING(100)
DIM str_getdata AS BOOLEAN
'MERGE =OFF
'LIMIT_BUFFERED=16
'VP_MODE=5
'IF KEY #tcp_port THEN
' CHANNEL_READ(tcp_port,str_rec)
'ENDIF
WHILE TRUE
'PRINT #0,TICKS
t=TICKS
jumpj(2)
wait_inpos()
paizhao:
' CHANNEL_WRITE(20,"T1")
' WA(500)
' WAIT UNTIL KEY #20
' IF KEY #20 THEN
' CHANNEL_READ(20,read)
' str_getdata(read,",",outdata)
'
' PRINT VAL(read)
'
' x=outdata(0)
'
' y=outdata(1)
'
' c=outdata(2)
' result=outdata(3)
' p_write_x(0,x)
' p_write_y(0,y)
' 'p_write_z(0,z)
' p_write_c(0,c)
' IF result=1.00 THEN
' jumpj(0)
' wait_inpos()
' OP(2,ON)
' ELSEIF result=0.00 THEN
' GOTO paizhao
' ENDIF
'
'
' ELSE
' GOTO paizhao
' ENDIF
jumpj(1)
wait_inpos()
OP(2,OFF)
PRINT #0, ABS(t-TICKS)/1000
'
''----------------------获取命令字符放在局部字符串变量l_str(0)--------
' IF KEY #tcp_port THEN
' l_str(0)=""
' CHANNEL_READ(tcp_port,str_rec)
' l_str(0)=str_rec
' PRINT #5,l_str(0)
' ENDIF
'' l_str(0) = "Pick,1,3,4,5,6,7"
'
'
''----------------------Home机器人回初始位------------------------
' IF LEFT(l_str(0),4)="Home" THEN
' GOSUB sub_home
' ENDIF
'
''----------------------机器人去吸嘴取料--------------------------
' IF LEFT(l_str(0),5)="Pick," THEN
' GOSUB sub_pick
' ENDIF
'
''----------------------机器人去平台放料位------------------------
' IF LEFT(l_str(0),3)="Put" THEN
' GOSUB sub_put
' ENDIF
WEND
'===================子程序:机器人回初始位====================
sub_home:
l_str(0)=""
OP(5,OFF):OP(11,ON):OP(8,OFF):OP(12,ON)
'WAIT UNTIL IN(7)=ON:GOSUB l10
'WAIT UNTIL IN(10)=ON:GOSUB l10
jumpj(0):wait_inpos()
movl(1):wait_inpos()
OP(6,OFF):OP(7,ON):OP(9,OFF):OP(10,ON)
WA(1)
OP(7,OFF):OP(10,OFF)
movl(0):wait_inpos()
GOSUB sub_str_pos
str_sec="Home,1,"+l_str(2)+CHR(13)+CHR(10)
CHANNEL_WRITE(tcp_port,str_sec)
RETURN
l10:
GOSUB sub_str_pos
str_sec="Home,2"+l_str(2)+CHR(13)+CHR(10)
CHANNEL_WRITE(tcp_port,str_sec)
RETURN
'===================子程序:获取当前坐标字符串====================
sub_str_pos:
DIM str_posx,str_posy,str_posz,str_posc AS STRING(100)
DIM current_pos AS TARGET
current_pos=ROBOT_LPOS
'获取当前坐标XYZC,并转换为字符串,保留3位小数
str_posx=STR(current_pos.x,3)
str_posy=STR(current_pos.y,3)
str_posz=STR(current_pos.z,3)
str_posc=STR(current_pos.w,3)
l_str(1)=str_posx+","+str_posy+","+str_posz+","+str_posc+CHR(13)+CHR(10)
l_str(2)=str_posx+","+str_posy+","+str_posz+","+str_posc
RETURN
'===================子程序:机器人去吸嘴取料======================
sub_pick:
str_getdata(l_str(0),",",d)
l_str(0)=""
b2=d(1)
OP(5,OFF):OP(11,ON) :OP(8,OFF):OP(12,ON)
PRINT #5,d(0),d(1),d(2),d(3),d(4)
'WAIT UNTIL IN(7)=ON
'WAIT UNTIL IN(10)=ON
SELECT_CASE b2
CASE 1
jumpl(10):wait_inpos()
OP(5,ON):OP(11,OFF)
' WAIT UNTIL IN(6)=ON
OP(6,ON)
'WAIT UNTIL IN(8)=ON
IF IN(8)=ON THEN
l_str(10)="1"
ELSE
l_str(10)="1"
ENDIF
OP(5,OFF):OP(11,OFF)
' WAIT UNTIL IN(7)=ON
CASE 2
jumpl(11):wait_inpos()
OP(8,ON):OP(12,OFF)
'WAIT UNTIL IN(9)=ON
OP(9,ON)
' WAIT UNTIL IN(11)=ON
IF IN(9)=ON THEN
l_str(10)="1"
ELSE
l_str(10)="0"
ENDIF
OP(8,OFF):OP(12,OFF)
'WAIT UNTIL IN(10)=ON
GOSUB sub_str_pos
str_sec="CalibT32"+","+l_str(10)+","+l_str(1)
CHANNEL_WRITE(tcp_port,str_sec)
END_CASE
RETURN
'===================子程序:机器人去平台放料位====================
sub_put:
'Put,b2,x,y,z,c
str_getdata(l_str(0),",",d)
b2=d(1)
l_str(0)=""
OP(5,OFF):OP(11,ON):OP(8,OFF):OP(12,ON)
'WAIT UNTIL IN(7)=ON
'WAIT UNTIL IN(10)=ON
SELECT_CASE b2
CASE 1
offset(d(2),d(3),d(4),d(5))'写入偏移值
movl(12):wait_inpos()
OP(5,ON):OP(6,ON):OP(11,OFF)
' WAIT UNTIL IN(6)=ON
CASE 2
offset(d(2),d(3),d(4),d(5))'写入偏移值
movl(13):wait_inpos()
OP(8,ON):OP(12,OFF)
' WAIT UNTIL IN(9)=ON
END_CASE
l_str(10)="1"
GOSUB sub_str_pos
str_sec="CalibT32"+","+l_str(10)+","+l_str(1)
CHANNEL_WRITE(tcp_port,str_sec)
RETURN
程序2hkjjdl
SELECT_CASE RUN_ERROR PROC(5)
CASE 30 '程序运行中
OP(3,OFF)'红灯灭
OP(5,OFF)'黄灯灭
OP(6,OFF)'蜂鸣器灭
OP(4,ON)'绿灯亮
CASE 31 '程序完成
OP(3,OFF)'红灯灭
OP(4,OFF)'绿灯灭
OP(6,OFF)'蜂鸣器灭
OP(5,ON)'黄灯亮
END_CASE
GOTO chongfu
'CO_WRITE_AXIS(8,3226,0,6,-1,
robot_select(0)
robot_speed(50,50,50)
jump_speed(50,20,20,50,20,20)
DIM b0,b1,b2,d0,d1,p_write_x,p_write_y,p_write_z,p_write_c AS INTEGER
DIM str_sec AS STRING(100)
DIM str_rec AS STRING(100)
DIM l_str AS STRING(100,100)
DIM d AS FLOAT(100)
DIM tcp_port AS INTEGER=20
DIM i AS INTEGER
DIM outdata AS FLOAT(500)
DIM t AS FLOAT
DIM x,y,z,c,result AS FLOAT
DIM read AS STRING(100)
DIM write AS STRING(100)
DIM str_getdata AS BOOLEAN
'MERGE =OFF
'LIMIT_BUFFERED=16
'VP_MODE=5
'IF KEY #tcp_port THEN
' CHANNEL_READ(tcp_port,str_rec)
'ENDIF
WHILE TRUE
'PRINT #0,TICKS
t=TICKS
jumpj(2)
wait_inpos()
paizhao:
'
''----------------------获取命令字符放在局部字符串变量l_str(0)--------
' IF KEY #tcp_port THEN
' l_str(0)=""
' CHANNEL_READ(tcp_port,str_rec)
' l_str(0)=str_rec
' PRINT #5,l_str(0)
' ENDIF
'' l_str(0) = "Pick,1,3,4,5,6,7"
'
'
''----------------------Home机器人回初始位------------------------
' IF LEFT(l_str(0),4)="Home" THEN
' GOSUB sub_home
' ENDIF
'
''----------------------机器人去吸嘴取料--------------------------
' IF LEFT(l_str(0),5)="Pick," THEN
' GOSUB sub_pick
' ENDIF
'
''----------------------机器人去平台放料位------------------------
' IF LEFT(l_str(0),3)="Put" THEN
' GOSUB sub_put
' ENDIF
'===================子程序:机器人回初始位====================
sub_home:
l_str(0)=""
OP(5,OFF):OP(11,ON):OP(8,OFF):OP(12,ON)
'WAIT UNTIL IN(7)=ON:GOSUB l10
'WAIT UNTIL IN(10)=ON:GOSUB l10
jumpj(0):wait_inpos()
movl(1):wait_inpos()
OP(6,OFF):OP(7,ON):OP(9,OFF):OP(10,ON)
WA(1)
OP(7,OFF):OP(10,OFF)
movl(0):wait_inpos()
GOSUB sub_str_pos
str_sec="Home,1,"+l_str(2)+CHR(13)+CHR(10)
CHANNEL_WRITE(tcp_port,str_sec)
RETURN
l10:
GOSUB sub_str_pos
str_sec="Home,2"+l_str(2)+CHR(13)+CHR(10)
CHANNEL_WRITE(tcp_port,str_sec)
RETURN
'===================子程序:获取当前坐标字符串====================
sub_str_pos:
DIM str_posx,str_posy,str_posz,str_posc AS STRING(100)
DIM current_pos AS TARGET
current_pos=ROBOT_LPOS
'获取当前坐标XYZC,并转换为字符串,保留3位小数
str_posx=STR(current_pos.x,3)
str_posy=STR(current_pos.y,3)
str_posz=STR(current_pos.z,3)
str_posc=STR(current_pos.w,3)
l_str(1)=str_posx+","+str_posy+","+str_posz+","+str_posc+CHR(13)+CHR(10)
l_str(2)=str_posx+","+str_posy+","+str_posz+","+str_posc
RETURN
'===================子程序:机器人去吸嘴取料======================
sub_pick:
str_getdata(l_str(0),",",d)
l_str(0)=""
b2=d(1)
OP(5,OFF):OP(11,ON) :OP(8,OFF):OP(12,ON)
PRINT #5,d(0),d(1),d(2),d(3),d(4)
'WAIT UNTIL IN(7)=ON
'WAIT UNTIL IN(10)=ON
SELECT_CASE b2
CASE 1
jumpl(10):wait_inpos()
OP(5,ON):OP(11,OFF)
' WAIT UNTIL IN(6)=ON
OP(6,ON)
'WAIT UNTIL IN(8)=ON
IF IN(8)=ON THEN
l_str(10)="1"
ELSE
l_str(10)="1"
ENDIF
OP(5,OFF):OP(11,OFF)
' WAIT UNTIL IN(7)=ON
CASE 2
jumpl(11):wait_inpos()
OP(8,ON):OP(12,OFF)
'WAIT UNTIL IN(9)=ON
OP(9,ON)
' WAIT UNTIL IN(11)=ON
IF IN(9)=ON THEN
l_str(10)="1"
ELSE
l_str(10)="0"
ENDIF
OP(8,OFF):OP(12,OFF)
'WAIT UNTIL IN(10)=ON
GOSUB sub_str_pos
str_sec="CalibT32"+","+l_str(10)+","+l_str(1)
CHANNEL_WRITE(tcp_port,str_sec)
END_CASE
RETURN
'===================子程序:机器人去平台放料位====================
sub_put:
'Put,b2,x,y,z,c
str_getdata(l_str(0),",",d)
b2=d(1)
l_str(0)=""
OP(5,OFF):OP(11,ON):OP(8,OFF):OP(12,ON)
'WAIT UNTIL IN(7)=ON
'WAIT UNTIL IN(10)=ON
SELECT_CASE b2
CASE 1
offset(d(2),d(3),d(4),d(5))'写入偏移值
movl(12):wait_inpos()
OP(5,ON):OP(6,ON):OP(11,OFF)
' WAIT UNTIL IN(6)=ON
CASE 2
offset(d(2),d(3),d(4),d(5))'写入偏移值
movl(13):wait_inpos()
OP(8,ON):OP(12,OFF)
' WAIT UNTIL IN(9)=ON
END_CASE
l_str(10)="1"
GOSUB sub_str_pos
str_sec="CalibT32"+","+l_str(10)+","+l_str(1)
CHANNEL_WRITE(tcp_port,str_sec)
RETURN
