一、motoplus适合函数



二、任务优先级



三、机器人位置的读取

1、编写读取变量和IO函数

important; overflow-wrap: break-word !important;">#include "motoPlus.h"important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">// for API INTERFACEimportant; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">// for API & FUNCTIONSimportant; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">extern long mpPutVarData(MP_VAR_DATA *, long);important; overflow-wrap: break-word !important;">extern long mpPutPosVarData(MP_POSVAR_DATA *, long);important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">extern long mpGetVarData(MP_VAR_INFO *sData, LONG* rData, LONG num);important; overflow-wrap: break-word !important;">extern long mpReadIO(MP_IO_INFO *sData, USHORT* rData, LONG num);important; overflow-wrap: break-word !important;">extern long mpWriteIO(MP_IO_DATA *sData, LONG num);important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">// for LOCAL DEFINITIONSimportant; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">/**************************************important; overflow-wrap: break-word !important;">**  useful function for application ***important; overflow-wrap: break-word !important;">***************************************/important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">STATUS GetBVar(UINT16 index, long *value)important; overflow-wrap: break-word !important;">{important; overflow-wrap: break-word !important;">  MP_VAR_INFO info;important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  info.usType = MP_RESTYPE_VAR_B;important; overflow-wrap: break-word !important;">  info.usIndex = index;important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  return mpGetVarData(&info, value, 1);important; overflow-wrap: break-word !important;">}important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">STATUS GetMultiBVar(UINT16 startIdx, long *value, long nbr)important; overflow-wrap: break-word !important;">{important; overflow-wrap: break-word !important;">  int i;important; overflow-wrap: break-word !important;">  MP_VAR_INFO info[20];important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  for(i=0; iimportant; overflow-wrap: break-word !important;">  {important; overflow-wrap: break-word !important;">    info[i].usType = MP_RESTYPE_VAR_B;important; overflow-wrap: break-word !important;">    info[i].usIndex = startIdx + i;important; overflow-wrap: break-word !important;">  }important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  return mpGetVarData(info, value, nbr);important; overflow-wrap: break-word !important;">}important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">STATUS SetBVar(UINT16 index, long value)important; overflow-wrap: break-word !important;">{important; overflow-wrap: break-word !important;">  MP_VAR_DATA info;important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  info.usType = MP_RESTYPE_VAR_B;important; overflow-wrap: break-word !important;">  info.usIndex = index;important; overflow-wrap: break-word !important;">  info.ulValue = value;important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  return mpPutVarData(&info, 1);important; overflow-wrap: break-word !important;">}important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">STATUS GetIVar(UINT16 index, long *value)important; overflow-wrap: break-word !important;">{important; overflow-wrap: break-word !important;">  MP_VAR_INFO info;important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  info.usType = MP_RESTYPE_VAR_I;important; overflow-wrap: break-word !important;">  info.usIndex = index;important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  return mpGetVarData(&info, value, 1);important; overflow-wrap: break-word !important;">}important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">STATUS SetIVar(UINT16 index, long value)important; overflow-wrap: break-word !important;">{important; overflow-wrap: break-word !important;">  MP_VAR_DATA info;important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  info.usType = MP_RESTYPE_VAR_I;important; overflow-wrap: break-word !important;">  info.usIndex = index;important; overflow-wrap: break-word !important;">  info.ulValue = value;important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  return mpPutVarData(&info, 1);important; overflow-wrap: break-word !important;">}important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">STATUS SetDVar(UINT16 index, long value)important; overflow-wrap: break-word !important;">{important; overflow-wrap: break-word !important;">  MP_VAR_DATA info;important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  info.usType = MP_RESTYPE_VAR_D;important; overflow-wrap: break-word !important;">  info.usIndex = index;important; overflow-wrap: break-word !important;">  info.ulValue = value;important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  return mpPutVarData(&info, 1);important; overflow-wrap: break-word !important;">}important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">STATUS SetMultiDVar(UINT16 startIdx, long *values, long nbr)important; overflow-wrap: break-word !important;">{important; overflow-wrap: break-word !important;">  int i;important; overflow-wrap: break-word !important;">  MP_VAR_DATA info[64];important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  nbr = (nbr > 64) ? 64 : nbr;important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  for(i=0; iimportant; overflow-wrap: break-word !important;">  {important; overflow-wrap: break-word !important;">    info[i].usType = MP_RESTYPE_VAR_D;important; overflow-wrap: break-word !important;">    info[i].usIndex = startIdx + i;important; overflow-wrap: break-word !important;">    info[i].ulValue = values[i];important; overflow-wrap: break-word !important;">  }important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  return mpPutVarData(info, nbr);important; overflow-wrap: break-word !important;">}important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">STATUS mpGetMultiDVar(UINT16 startIdx, long *values, long nbr)important; overflow-wrap: break-word !important;">{important; overflow-wrap: break-word !important;">  int i;important; overflow-wrap: break-word !important;">  MP_VAR_INFO info[64];important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  nbr = (nbr > 64) ? 64 : nbr;important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  for(i=0; iimportant; overflow-wrap: break-word !important;">  {important; overflow-wrap: break-word !important;">    info[i].usType = MP_RESTYPE_VAR_D;important; overflow-wrap: break-word !important;">    info[i].usIndex = startIdx + i;important; overflow-wrap: break-word !important;">  }important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  return mpGetVarData(info, values, nbr);important; overflow-wrap: break-word !important;">}important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">STATUS GetXVar(UINT16 type, UINT16 index, char *value)important; overflow-wrap: break-word !important;">{important; overflow-wrap: break-word !important;">  MP_VAR_INFO info;important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  info.usType = type;important; overflow-wrap: break-word !important;">  info.usIndex = index;important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  return mpGetVarData(&info, (long *)value, 1);important; overflow-wrap: break-word !important;">}important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">STATUS GetIo(UINT32 ioAddr, USHORT *signal)important; overflow-wrap: break-word !important;">{important; overflow-wrap: break-word !important;">  MP_IO_INFO nfo;important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  nfo.ulAddr = ioAddr;important; overflow-wrap: break-word !important;">  return mpReadIO(&nfo, signal, 1);important; overflow-wrap: break-word !important;">}important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">STATUS SetIo(UINT32 ioAddr, USHORT signal)important; overflow-wrap: break-word !important;">{important; overflow-wrap: break-word !important;">  MP_IO_DATA dta;important; overflow-wrap: break-word !important;">  dta.ulAddr = ioAddr;important; overflow-wrap: break-word !important;">  dta.ulValue = signal;important; overflow-wrap: break-word !important;">  return mpWriteIO(&dta, 1);important; overflow-wrap: break-word !important;">}important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">STATUS SetMultiIo(UINT32 ioStartAddr, USHORT *signal, long nbr)important; overflow-wrap: break-word !important;">{important; overflow-wrap: break-word !important;">  MP_IO_DATA dta[20];important; overflow-wrap: break-word !important;">  int i;important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  if( nbr > 20 )important; overflow-wrap: break-word !important;">    return ERROR;important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  for(i=0; iimportant; overflow-wrap: break-word !important;">  {important; overflow-wrap: break-word !important;">    dta[i].ulAddr = ioStartAddr+i;important; overflow-wrap: break-word !important;">    dta[i].ulValue = signal[i];important; overflow-wrap: break-word !important;">  }important; overflow-wrap: break-word !important;">  return mpWriteIO(dta, nbr);important; overflow-wrap: break-word !important;">}important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">STATUS WriteIO(UINT32 addr, UINT16 value)important; overflow-wrap: break-word !important;">{important; overflow-wrap: break-word !important;">  MP_IO_DATA sData;important; overflow-wrap: break-word !important;">  sData.ulAddr = addr;important; overflow-wrap: break-word !important;">  sData.ulValue = value;important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  return mpWriteIO(&sData, 1);important; overflow-wrap: break-word !important;">}important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">STATUS ReadIO(UINT32 addr, UINT16* value)important; overflow-wrap: break-word !important;">{important; overflow-wrap: break-word !important;">  MP_IO_INFO sData;important; overflow-wrap: break-word !important;">  sData.ulAddr = addr;important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !important;">  return mpReadIO (&sData, value, 1);
 

2、使用信号量起到定时器的作用

important; overflow-wrap: break-word !important;">
important; overflow-wrap: break-word !
• important;">//每50ms给出一个信号量important; overflow-wrap: break-word !important;">mpSemGive(semid);//给出一个信号量important; overflow-wrap: break-word !important;">mpTaskDelay(50);//任务延时important; overflow-wrap: break-word !important;">//接收信号量important; overflow-wrap: break-word !important;">status = mpSemTake(semid, WAIT_FOREVER);//接收信号量，一直等待important; overflow-wrap: break-word !important;">if (status == ERROR)important; overflow-wrap: break-word !important;">{important; overflow-wrap: break-word !important;"> printf("semTake Error![%d]\n", run_cnt);important; overflow-wrap: break-word !important;">}

3、采集机器人坐标

•  important; overflow-wrap: break-word !important;">
•  important; overflow-wrap: break-word !

  important;">//接收信号量以后采集机器人坐标位置，发送给上位机important; overflow-wrap: break-word !important;">mp_ctrl_grp_send_data.sCtrlGrp = 0;  //  R1,ROBOT1important; overflow-wrap: break-word !important;">//mpGetCartPos读取成功，返回0，读取不成功，返回-1important; overflow-wrap: break-word !important;">//mpTaskDelay(80);//隔50毫秒读取机器人位置并发送important; overflow-wrap: break-word !important;">if (mpGetCartPos(&mp_ctrl_grp_send_data, &mp_cart_pos_rsp_data) != 0)  important; overflow-wrap: break-word !important;">{important; overflow-wrap: break-word !important;">  puts("get cart pos error");important; overflow-wrap: break-word !important;">  break;important; overflow-wrap: break-word !important;">}important; overflow-wrap: break-word !important;">// Copy current position to P variableimportant; overflow-wrap: break-word !important;">memset(&mp_posvar_data.ulValue[0], 0, (sizeof(long) * 10 )); // all clearimportant; overflow-wrap: break-word !important;">memcpy(&mp_posvar_data.ulValue[2], &mp_cart_pos_rsp_data.lPos[0], (sizeof(long) * 6));//复制数组里面的六组位置变量  important; overflow-wrap: break-word !important;">memcpy(&pos_buff[0], &mp_cart_pos_rsp_data.lPos[0], (sizeof(long) * 6));//复制数组里面的六组位置变量 important; overflow-wrap: break-word !important;">//printf("Exit mpUsrRoot!");important; overflow-wrap: break-word !important;">/*printf("pos_buff0 = (%d)", pos_buff[0]);important; overflow-wrap: break-word !important;">printf("pos_buff1 = (%d)", pos_buff[1]);important; overflow-wrap: break-word !important;">printf("pos_buff2 = (%d)", pos_buff[2]);important; overflow-wrap: break-word !important;">printf("pos_buff3 = (%d)", pos_buff[3]);important; overflow-wrap: break-word !important;">printf("pos_buff4 = (%d)", pos_buff[4]);important; overflow-wrap: break-word !important;">printf("pos_buff5 = (%d)", pos_buff[5]);*/important; overflow-wrap: break-word !important;">
  important; overflow-wrap: break-word !important;">memcpy(&position_buff[0], &pos_buff[0], (sizeof(long) * 6));//复制数组里面的六组位置变量 important; overflow-wrap: break-word !important;">position_bytesNum = sizeof(pos_buff);important; overflow-wrap: break-word !important;">//printf("send(%d)",position_bytesNum);important; overflow-wrap: break-word !important;">position_bytesSend = mpSend(sockHandle, position_buff, position_bytesNum, 0);//发送数据，返回发送的位数//发送150important; overflow-wrap: break-word !important;">
  important; overflow-wrap: break-word !important;">if (position_bytesSend != position_bytesNum)//发送的不完全important; overflow-wrap: break-word !important;">  {important; overflow-wrap: break-word !important;">     printf("Error : send() rc = %d\n", position_bytesSend);important; overflow-wrap: break-word !important;">  } important; overflow-wrap: break-word !important;">mp_posvar_data.ulValue[0] |= (long)(mp_cart_pos_rsp_data.sConfig << 8);//相对位置信息左移八位

•  
•  四、总结

注意事项：
（一）机器人坐标系是long 型数据，发送给上位机时需要转化为字节型数据
常用的方法有itoa和sprintf
itoa的使用方法：
itoa(pos_buff[1],sendPositionBuff_Y,10);
//将pos_buff[1]中数据转化到sendPositionBuff_Y的字符串数据
***个参数是要转换的数字，第二个参数是要写入转换结果的目标字符串，第三个参数是转移数字时所用的基数，比如10表示10进制，2表示2进制。

sprintf的用法
sprintf(str, "%x", 100); //将100转为16进制表示的字符串

（二）上位机接收数据较容易的方法是使用memcpy
memcpy(&sendPositionBuff[0], &pos_buff[0], (sizeof(pos_buff)));//复制数组里面的六组位置变量

原型如下：
void *memcpy(void *dest, void *src, unsigned int count);
由src所指内存区域复制count个字节到dest所指内存区域。

（三）上位机解析发送的机器人六自由度坐标，在很短的时间内造成程序终止，分析原因认为上位机的buffer内存溢出，需要花时间解决。

由于焊接机器人在实际使用过程中会发生与实际示教路径之间的偏移，需要在工业机器人焊接端放置激光视觉传感器进行误差跟踪，并将误差信息上传给机器人控制器，对机器人工作路径进行实时纠偏。

---