当前位置： 首页 > news >正文

安川机器人motoplus二次开发实现socket 变量读写 SKILLSND命令接收 轨迹实时修正功能

news 2025/10/23 8:10:41

每个安川机器人控制柜可以导入一个.out文件在后台进行运行,在motopluside中编写代码并编译即可生成out文件

//mpMain.c
#include "motoPlus.h"
#include "string.h"
#include "math.h"
#include <stdio.h>
#include <stdlib.h>int  SetApplicationInfo();
void mpTask1();
void mpTask2();
void mpTask3();
void mpTask4();//GLOBAL DATA DEFINITIONS
int nTaskID1;
int nTaskID2;
int nTaskID3;
int nTaskID4;
//被使用的函数要在此声明
extern STATUS GetBVar(UINT16 index, long *value);
extern STATUS SetBVar(UINT16 index, long value);
extern STATUS GetMultiBVar(UINT16 startIdx, long *value, long nbr);
extern STATUS SetMultiBVar(UINT16 startIdx, long *values, long nbr);
extern STATUS GetPosVar(USHORT Type, USHORT Index, LONG *VarType, LONG *Posture, LONG *ToolNo, LONG *UFno, LONG Coord[8]);
extern STATUS PutPosVar(USHORT Type, USHORT Index, LONG VarType, LONG Posture, LONG ToolNo, LONG UFno, LONG Coord[8]);
extern STATUS GetPos(SHORT RobotNo, SHORT Frame, SHORT ToolNo, LONG Pos[MAX_CART_AXES_EX]);
extern STATUS SetSVar(UINT16 index, char str[STR_VAR_SIZE+1]);void mpUsrRoot(int arg1, int arg2, int arg3, int arg4, int arg5, int arg6, int arg7, int arg8, int arg9, int arg10)
{int rc;//TODO: Add additional intialization routines.        //Creates and starts a new task in a seperate thread of execution.//All arguments will be passed to the new task if the function//prototype will accept them.//优先级  堆栈大小  入口函数指针nTaskID1 = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, (FUNCPTR)mpTask1,arg1, arg2, arg3, arg4, arg5, arg6, arg7, arg8, arg9, arg10);nTaskID2 = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, (FUNCPTR)mpTask2,arg1, arg2, arg3, arg4, arg5, arg6, arg7, arg8, arg9, arg10);nTaskID3 = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, (FUNCPTR)mpTask3,arg1, arg2, arg3, arg4, arg5, arg6, arg7, arg8, arg9, arg10);nTaskID4 = mpCreateTask(MP_PRI_IP_CLK_TAKE, MP_STACK_SIZE, (FUNCPTR)mpTask4,arg1, arg2, arg3, arg4, arg5, arg6, arg7, arg8, arg9, arg10);//Set application information.rc = SetApplicationInfo();//Ends the initialization task.mpExitUsrRoot;
}//Set application information.
int SetApplicationInfo(void)
{MP_APPINFO_SEND_DATA    sData;MP_STD_RSP_DATA         rData;int                     rc;memset(&sData, 0x00, sizeof(sData));memset(&rData, 0x00, sizeof(rData));strncpy(sData.AppName,  "Default Application",  MP_MAX_APP_NAME);strncpy(sData.Version,  "0.00",                 MP_MAX_APP_VERSION);strncpy(sData.Comment,  "MotoPlus Application", MP_MAX_APP_COMMENT);rc = mpApplicationInfoNotify(&sData, &rData);return rc;
}void mpTask1(void)
{//TODO: Add the code for this task//********************************读取B000写入到B001********************************
//	long b0;
//	GetBVar(0,&b0);//起始位 值的地址
//	SetBVar(1,b0);//地址 值//********************************多个B变量的读写********************************
//	long BValue[5];
//	GetMultiBVar(0,&BValue,5);//起始位 值的地址 个数
//	SetMultiBVar(6,BValue,5);//地址 值  个数//********************************读取P位置变量********************************
//	LONG VarType,Posture,ToolNo,UFno,pCoord[8];
//	//对象为机器人  第几个P变量  坐标类型  姿态 工具号 用户号 p变量的6个值
//	GetPosVar(5,0,&VarType,&Posture,&ToolNo,&UFno,&pCoord);//********************************写入P位置变量********************************
//	LONG scoord1[8];
//	//对象为机器人 P001 17代表机器人坐标 姿态为0  工具号1 用户号0 0表示不使用用户坐标系
//	//16 关节坐标  17 直角坐标  18工具坐标 19用户坐标
//	scoord1[0]=10000;//X10.000mm
//	scoord1[1]=10000;//Y10.000mm
//	scoord1[2]=10000;//Z10.000mm
//	scoord1[3]=10000;//RX1.0000度
//	scoord1[4]=10000;//RY1.0000度
//	scoord1[5]=10000;//RZ1.0000度
//	while(1)
//	{
//		PutPosVar(5,3,17,0,1,0,&scoord1);
//	}//********************************获取当前位置********************************
//	LONG pos[MAX_CART_AXES_EX];
//	GetPos(0,1,0,&pos);//0=R1 1=机器人坐标 0=TOOL0 
//******************************************************************************//	while(1)//不断将当前位置写入到标量P003中
//	{
//		LONG pos[MAX_CART_AXES_EX];
//		GetPos(0,1,0,&pos);//0=R1 0=关节 1=机器人坐标 2=工具坐标 3=用户坐标 0=TOOL(仅仅当前一个参数为2时生效?) 
//	    //对象为机器人 Pxxx 17代表机器人坐标 姿态为0  工具号0 用户号0
//		PutPosVar(5,3,17,0,0,0,&pos);
//	}}void mpTask2(int arg1, int arg2)
{
//	const int SeverPort = 8800;  // 定义服务器监听端口为8800
//	int SeverSocket;   //服务器套接字
//	char error[1000];  //写入到S变量的报错信息
//	int AcceptHandle;  // 用于存储接受的客户端连接句柄
//	int status;  // 用于存储函数调用状态
//	struct sockaddr_in serverSockAddr;  // 定义服务器地址结构体
//    struct sockaddr_in cilentSockAddr;  // 定义客户端地址结构体
//    int bytesRecv;
//    LONG BUFF_MAX=1023;
//    unsigned char buffrev[BUFF_MAX + 1];
//	while(1)
//	{
//		// 创建TCP套接字(AF_INET表示IPv4，SOCK_STREAM表示TCP协议 0是协议编号 固定为0)
//	    SeverSocket = mpSocket(AF_INET, SOCK_STREAM, 0);
//	    if (SeverSocket < 0) // 检查套接字创建是否成功
//	    {  
//	        memset(error, '\0', sizeof(char) * 1000);
//            strcpy(error, "Can not create socket");
//            SetSVar(0, error);
//            continue;
//	    }
//	    
//	    // 配置服务器地址结构体
//	    memset(&serverSockAddr, 0, sizeof(serverSockAddr));//指针 被填充的值 填充字节数
//	    serverSockAddr.sin_family = AF_INET;  // 使用IPv4
//	    serverSockAddr.sin_addr.s_addr = INADDR_ANY;  // 监听所有网络接口
//	    serverSockAddr.sin_port = mpHtons(SeverPort);  // 设置端口号(转换为网络字节序)
//	    
//	    // 绑定套接字到指定端口
//	    status = mpBind(SeverSocket, (struct sockaddr*)&serverSockAddr, sizeof(serverSockAddr));
//	    if (status < 0) // 检查绑定是否成功
//	    {
//	    	memset(error, '\0', sizeof(char) * 1000);
//            strcpy(error, "Bind socket fail");
//            SetSVar(0, error);
//            mpClose(SeverSocket);// 失败则关闭套接字
//			continue;
//	    }
//	    
//	    // 开始监听连接请求(SOMAXCONN表示最大连接数)
//	    status = mpListen(SeverSocket, SOMAXCONN);
//	    if (status < 0) // 检查监听是否成功
//	    {
//	    	memset(error, '\0', sizeof(char) * 1000);
//            strcpy(error, "listen port fail");
//            SetSVar(0, error);
//            mpClose(SeverSocket);// 失败则关闭套接字
//			continue;
//	    }
//	    
//	    // 接受客户端连接请求
//	    memset(&cilentSockAddr, 0, sizeof(cilentSockAddr));
//	    AcceptHandle = mpAccept(SeverSocket, (struct sockaddr*)&cilentSockAddr, sizeof(cilentSockAddr));
//	    if (AcceptHandle < 0)  // 如果接受连接失败
//	    { 
//	        memset(error, '\0', sizeof(char) * 1000);
//            strcpy(error, "accept port fail");
//            SetSVar(0, error);
//            mpClose(SeverSocket);// 失败则关闭套接字
//			continue;
//	    }
//	    while(1)
//	    {
//	    	bytesRecv = mpRecv(AcceptHandle, buffrev, BUFF_MAX, 0);//套接字 存储接收信息的变量指针  指定前者最大接收长度  0
//	    	if (bytesRecv < 0)
//	        {
//	            memset(error, '\0', sizeof(char) * 1000);
//	            strcpy(error, "Failed to Recv message");
//	            SetSVar(0, error);
//	            mpClose(SeverSocket);// 失败则关闭套接字
//				break;
//	        }
//	        status=mpSend(AcceptHandle, buffrev, bytesRecv, 0);
//	    	if (status < 0)
//	        {
//	            memset(error, '\0', sizeof(char) * 1000);
//	            strcpy(error, "Failed to Send message");
//	            SetSVar(0, error);
//	            mpClose(SeverSocket);// 失败则关闭套接字
//				break;
//	        }
//	    }
//
//	}
//	//mpTaskDelay(1000);//TODO: Add the code for this task
}int command_no;
void mpTask3(void)
{char error[1000];  //写入到S变量的信息SYS2MP_SENS_MSG msg;int status;memset(&msg,CLEAR,sizeof(SYS2MP_SENS_MSG));while(1)//和FOREVER有啥区别  FOREVER是for(;;){mpEndSkillCommandProcess(MP_SL_ID1,&msg);//结束处理指令status=mpReceiveSkillCommand(MP_SL_ID1,&msg);//接收通过SKILLSND发送的命令  这个命令是安川程序中调用的 可以从程序发送指令给后端.out文件if(status<0)continue;switch(msg.main_comm){case MP_SKILL_COMM:switch(msg.sub_comm){case MP_SKILL_SEND:{if(strcmp(msg.cmd,"START")==0)//如果消息是START{command_no=1;//设置轨迹补偿是否启动变量为真memset(error, '\0', sizeof(char) * 1000);//将空赋值给error字符串 相当于赋值前的初始化strcpy(error, "START");//将error赋值为errorSetSVar(1, error);//设置S变量1的值  在示教器的S1变量将会显示error字符串的内容}else if(strcmp(msg.cmd,"END")==0){command_no=0;memset(error, '\0', sizeof(char) * 1000);strcpy(error, "END");SetSVar(1, error);}}break;case MP_SKILL_END:{}break;}}}}int cnt=0;
void mpTask4(void)
{char error[1000];  //写入到S变量的信息int ret;LONG M0;SYS2MP_SENS_MSG msg;MP_POS_DATA corrpath_src_p;memset(&msg,CLEAR,sizeof(SYS2MP_SENS_MSG));//初始化一下变量memset(&corrpath_src_p,CLEAR,sizeof(MP_POS_DATA));corrpath_src_p.ctrl_grp = 1;//R1corrpath_src_p.grp_pos_info[0].pos_tag.data[0] = 0x3f;//有效轴指定6个轴 0011_1111corrpath_src_p.grp_pos_info[0].pos_tag.data[2] = 0;//不使用corrpath_src_p.grp_pos_info[0].pos_tag.data[3] = MP_CORR_RF_DTYPE;//机器人坐标系
//	corrpath_src_p.grp_pos_info[0].pos_tag.data[3] = MP_CORR_UF_DTYPE;//用户坐标系 
//	corrpath_src_p.grp_pos_info[0].pos_tag.data[4] = 0;//UF(1) 用户坐标系是1开始 对应这里的标号0corrpath_src_p.grp_pos_info[0].pos[0] = 0;//X  值初始化 单位应该是0.001mmcorrpath_src_p.grp_pos_info[0].pos[1] = 0;//Ycorrpath_src_p.grp_pos_info[0].pos[2] = 0;//Zcorrpath_src_p.grp_pos_info[0].pos[3] = 0;//RXcorrpath_src_p.grp_pos_info[0].pos[4] = 0;//RYcorrpath_src_p.grp_pos_info[0].pos[5] = 0;//RZwhile(1){mpClkAnnounce(MP_INTERPOLATION_CLK);//按插补周期执行以下处理switch(command_no){case 0:{cnt=0;}break;case 1:{M0=1*1000;//单位:0.001mm/sif(cnt++>=49)//每50个插补周期进行一次修正值计算 一哥插补周期4ms?{corrpath_src_p.grp_pos_info[0].pos[1]=M0*1/5;//每0.2s(4ms*50)在Y方向上修正M0/5ret=mpMeiPutCorrPath(MP_SL_ID1,&corrpath_src_p);//应该是执行一次补偿一次if(ret<0){memset(error, '\0', sizeof(char) * 1000);strcpy(error, "Path ERROR");SetSVar(2, error);}else if(ret==0){memset(error, '\0', sizeof(char) * 1000);strcpy(error, "Path Running");SetSVar(2, error);}cnt=0;}}break;}}

http://www.dtcms.com/a/515664.html

相关文章：

  • 社交网站 备案wordpress硬件接口
  • Redis未授权访问漏洞：从原理到高级利用
  • 项目实践3—全球证件智能识别系统（Qt客户端开发+FastAPI后端人工智能服务开发）
  • 【LeetCode】87. 扰乱字符串
  • React学习路径与实践指南
  • Linux系统的ARM库移植
  • Flutter 16KB 页面大小支持配置
  • gateface做网站中国建筑查询网
  • No039:谦逊的智慧——当DeepSeek深知所知有限
  • 【完整源码+数据集+部署教程】【天线＆空中农业】蜜蜂检测系统源码＆数据集全套：改进yolo11-ASF
  • 作业控制和后台运行
  • 网站网站建设企业wordpress如何设置阅读权限
  • (PCGU) Probability-based Global Cross-modal Upsampling for Pansharpening
  • 物联网联动策略表结构设计
  • 一文学会标准库USART
  • 5-6〔OSCP ◈ 研记〕❘ SQL注入攻击▸自动化工具SQLMap
  • Linux进程间通信：深入解析PV操作及其同步机制
  • Servlet 实例详解
  • 个人备案网站盈利动画制作网页
  • 网站建设饱和了吗上海市建设市场信息服务平台
  • 个人备案域名做企业网站wow slider wordpress
  • 【OPENGL ES 3.0 学习笔记】第九天：缓存、顶点和顶点数组
  • 2510rs,rust,1.85
  • 深度学习（13）-PyTorch 数据转换
  • rocketmq实现取消超时订单？兜底方案？
  • Linux如何安装使用Rust指南
  • 田块处方图可视化（PyQt5）
  • Rust算法复杂度-大O分析
  • 2510rs,rust清单4
  • 大型网站开发考试移动商城的推广方法