定时发送力传感器数据至机器人

每次完成一个demo都需要记下小笔记，防止忘记，实验如图：


第一张图中是红绿相接的部分是通讯结果，$表示接受完整，这里力传感器的频率为100hz,通过定时器每10ms向socket发送数据（只能有一个定时器运行！！不然发送时间不准，有滞后），机器人的同步运行周期设为了0.008s,最小控制周期是4ms,同步运行周期必须是其整数倍。


在下位机的start()函数里创建两个同步任务，一个用来读取socket,一个用来更新机器人示教器上的UI界面
本次实验以机器人为客户端，地址如图，电脑为服务端，监听端口号1000

下位机代码如下：
从start()开始，（同步运行任务好像有行数限制，所以尽量多用子函数，否则会报错！！）

begin
  userPage()
  cls()
  gotoxy(0,0)
  put("Socket sensorRead测试程序：")
  taskCreateSync "readSocket",0.008,bSupervisor,readSocket(nError,nNum_FX,nNum_FY,nNum_FZ,nNum_TX,nNum_TY,nNum_TZ)
  taskCreateSync "UI",0.008,bSupervisor,UI(nNum_FX,nNum_FY,nNum_FZ,nNum_TX,nNum_TY,nNum_TZ)
  
end

readsocket()

begin
  // This program gets the parts sent by the other robot. 
  //  The parts arrive via a socket on a telegram with the format
  //  {ID=i,X=x.xxx,Y=y.yyy,RZ=r.rrr,L=l.llllll},
  //  where "{" is the initial character, "i" is the ID of the
  //  detected object, "x.xxx" is the value on X, "y.yyy" is the
  //  value on Y, "r.rrr" is the value on RZ, "l.llllll" is the
  //  latched encoder value, "," is the separator character and "}"
  //  is the end character. At the end, it sends the character "$" 
  //  as confirmation of good reception.
  // The format of the telegram is defined at the program 
  //  sendToNextRobot at the Motion library.
  //
  // Input Param
  //  none       : 
  //
  // Output Param
  //  x_nID         : ID of the detected object
  //  x_trObjectPos : Transformation of the detected object
  //  x_nEncLatchVal: Encoder latched value of the detected object
  //  x_nError      :
  //              0 = no error, one part detected.
  //             >1 = Number of parameters in error with in the
  //                   telegram read. It wasn't possible to fully
  //                   convert into a numerical value one of the
  //                   parameter within the telegram.
  //
  //{X=x.xxx,Y=y.yyy,Z=z.zzz,Rx=rx.xxx,Ry=ry.yyy,RZ=rz.zzz}
  //
  //----------------------------------------------------------------
  //
  //Init Variables
 x_nNum_FX = 0
 x_nNum_FY = 0
 x_nNum_FZ = 0
 x_nNum_TX = 0
  x_nNum_TY = 0
   x_nNum_TZ = 0
   
 
  x_nError=0
  //
  
  
  while true
     // Looks for int Character
  call waitCharOnSk("{")
  //
  // Looks for beginig of first value
  call waitCharOnSk("=")
  //
  // Recover X value
  call readValFromSk(",",x_nNum_FX,x_nError)
  if x_nError>0
    logMsg("(readSocket)>>"+sErrorCode[0])
  endIf
  //
  // Looks for beginig of next value
  call waitCharOnSk("=")
  //
  // Recover Y value
  call readValFromSk(",",x_nNum_FY,x_nError)
  if x_nError>0
    logMsg("(readSocket)>>"+sErrorCode[1])
  endIf
  //
  // Looks for beginig of next value
  call waitCharOnSk("=")
  //
  // Recover Z value
  call readValFromSk(",",x_nNum_FZ,x_nError)
  if x_nError>0
    logMsg("(readSocket)>>"+sErrorCode[2])
  endIf
  //
  // Looks for beginig of next value
  call waitCharOnSk("=")
  //
  //Recover Z value
  call readValFromSk(",",x_nNum_TX,x_nError)
  if x_nError>0
    logMsg("(readSocket)>>"+sErrorCode[3])
  endIf
  //
  // Looks for beginig of next value
  call waitCharOnSk("=")
  //
  //Recover Z value
  call readValFromSk(",",x_nNum_TY,x_nError)
  if x_nError>0
    logMsg("(readSocket)>>"+sErrorCode[4])
  endIf
  // 
  // Looks for beginig of next value
  call waitCharOnSk("=")
  //
  //Recover Z value
  call readValFromSk("}",x_nNum_TZ,x_nError)
  if x_nError>0
    logMsg("(readSocket)>>"+sErrorCode[5])
  endIf
  //
  //
  // Sends the character "$" as confirmation of good reception.
  sioSet(skFromPc,asc("$",0))
  
  delay(0)
  endWhile
 
  //
end

readvalFromSk()

begin
  // This program reads character by character on the socket until
  //  it finds a separator character. All the characters founded before
  //  this separator character are transformed on an integer value.
  //  The socket must be a socket server with timeout=0
  //
  // Input Param
  //  x_sCharacter : Character to find
  //
  // Output Param
  //  x_nValue     : Integer value read from the socket
  //  x_nError     :
  //             0 = No error
  //            +1 = Not possible to convert the string read into
  //                  a numerical value, or the string doesn't have
  //                  the right format.
  //
  //----------------------------------------------------------------
  //
  l_sStringRead=""
  l_sCharRead=""
  //
  // Read character by character until finds specific character.
  while l_sC