机器人-新松

通信接收数据分解脚本

data={}
str_pose=""
k=0
value=0
VisionPose={}
RobPose={}
Camera_pose1={}
Camera_pose2={}
Capture_socket=""
socket_write("CAPTURE #","camera",3000)
Capture_socket=socket_read_string(125,"","#","camera",10000)
--判断拍照是否结果是否OK
if(Capture_socket=="0") then
 else
--如果OK接受95个数据，分19次接收
while(k<19)
--没有收到数据，就一直读，直到读到数据
do
  --str_num接收数组，格式为（num1，num2，num3）注意括号也算
  --str_pose=socket_read_str_num("camera",100)
  
  --接收字符串，用#号结尾
  socket_write("GET_STRING 4#","camera",3000)
  str_pose=socket_read_string(255,"","#","camera",3000)
     --没接收5个数据，进行分割
  data=str_split(str_pose,",")
    log("data",data)
    num=0
   --分割数据进行组合pose数组
    while(num<5) do
      value=k*5+num+1
       log("value",value)
   if(value<49)then
   Camera_pose1[value]= {str2num(data[6*num+1]),str2num(data[6*num+2]),str2num(data[6*num+3]),str2num(data[6*num+4]),str2num(data[6*num+5]),str2num(data[6*num+6])}
         log("Set_RVS_Return_Pose1 mm", Camera_pose1[value])
      else
       Camera_pose2[value-48]= {str2num(data[6*num+1]),str2num(data[6*num+2]),str2num(data[6*num+3]),str2num(data[6*num+4]),str2num(data[6*num+5]),str2num(data[6*num+6])}
         log("Set_RVS_Return_Pose2 mm",Camera_pose2[value-48])
      end
      
      num=num+1
      end
  --log("read_Vision_Return",str_pose)
  k=k+1
end
--分割字符串

--data=str_split(str_pose," ")


  

--log("Get_RVS_Return_Pose mm", VisionPose)
--视觉返回 单位mm 角度
--返回格式（X,Y,Z,RX,RY,RZ,1）
--分别传给两个Camera进行分组

  log("Get_RVS_Return_Pose1-1 mm", Camera_pose1[1])
  log("Get_RVS_Return_Pose2-1 mm", Camera_pose2[1])
 
--spline(Camera_pose1,0.1,1,"default","default")
--spline(Camera_pose2,0.1,1,"default","default")
--RobPose[1]=data[1] --/1000
--RobPose[2]=data[2] --/1000

  --RobPose[3]=data[3] --/1000
--RobPose[4]=data[4] --/57.29578
----RobPose[5]=data[5] --/57.29578
--RobPose[6]=data[6] --/57.29578
--单位转换
--log("Get_RVS_Return_Pose m", RobPose)
set_value("g_RVS_POSE1",Camera_pose1[1] )
  Camera_poseone=pose_trans({0,0,0.003,0,0,0},Camera_pose1[1])
  
 movel(Camera_poseone,0.1,1,0,{},"default","default")
  --movel(Camera_pose1[2],0.5,1,0,{},"new123","default")
 -- movel(Camera_pose1[3],0.5,1,0,{},"new123","default")
  --movel(Camera_pose1[4],0.5,1,0,{},"new123","default")
  --movel(Camera_pose1[5],0.5,1,0,{},"new123","default")
 -- movel(Camera_pose1[6],0.5,1,0,{},"new123","default")
  --movel(Camera_pose1[7],0.5,1,0,{},"new123","default")
 --movel(Camera_pose1[8],0.5,1,0,{},"new123","default")
  --movel(Camera_pose1[9],0.5,1,0,{},"new123","default")
 -- movel(Camera_pose1[10],0.5,1,0,{},"new123","default")
  --movel(Camera_pose1[11],0.5,1,0,{},"new123","default")
 -- movel(Camera_pose1[12],0.5,1,0,{},"new123","default")
  --spline({Camera_pose1[1],Camera_pose1[2],Camera_pose1[3],Camera_pose1[4],Camera_pose1[5]},0.5,1,"new123","default")
log("Camera_pose1: ",Camera_pose1[1])
log("Camera_pose2: ",Camera_pose1[2])
spline(Camera_pose1,0.1,1,"default","default")
spline(Camera_pose2,0.1,1,"default","default")
--刷新系统变量基准点数据，机器人默认单位为米弧度
--视觉返回的pose作为基准点, 存储在系统变量g_VisionPose0中
  end