6轴关节机械手仿真和控制QtQuick3D 和 CodeSys Robotics
一、概述
本工程利用QtQuick3D做上位机 ，基于CodeSys的控制器，使用 Robotics的库，进行6轴垂直机械手仿真和控制。
二、程序
2.1QtQuick程序
2.1.1程序组织
2.1.2代码
2.1.2.1 pro文件
QT += quick quick3d network
CONFIG += qmltypes
QML_IMPORT_NAME = RobotArm
QML_IMPORT_MAJOR_VERSION = 1
SOURCES += \
armgeometry.cpp \
main.cpp \
plccomm.cpp
resources.files = main.qml \
Toggle.ui.qml
resources.prefix = /$${TARGET}
RESOURCES += resources
# Additional import path used to resolve QML modules in Qt Creator's code model
QML_IMPORT_PATH =
# Additional import path used to resolve QML modules just for Qt Quick Designer
QML_DESIGNER_IMPORT_PATH =
# Default rules for deployment.
qnx: target.path = /tmp/$${TARGET}/bin
else: unix:!android: target.path = /opt/$${TARGET}/bin
!isEmpty(target.path): INSTALLS += target
HEADERS += \
armgeometry.h \
plccomm.h
DISTFILES += \
Toggle.ui.qml2.1.2.2 armgeometry.h
#ifndef ARMGEOMETRY_H
#define ARMGEOMETRY_H
#include
#include
#include
#include
#include
struct cubePoint {
float x;
float y;
float z;
};
class ArmGeometry: public QQuick3DGeometry
{
Q_OBJECT
QML_NAMED_ELEMENT(ArmGeometry)
Q_PROPERTY(QString sharpData READ sharpData WRITE setSharpData NOTIFY sharpDataChanged) //暴露QML的属性,sharpData
public:
ArmGeometry(QQuick3DObject *parent = nullptr);
QString sharpData() { return m_sharpData; }
void setSharpData(QString sharpData);
signals:
void sharpDataChanged();
private:
void calculateGeometry(); //构造Geometry
void updateData(); //重绘函数
void cubePointAppend(cubePoint p); //添加立方体点
void clearCpArr(); //复位立方体顶点数据
void setCpArr(int id,float x, float y, float z); //设置立方体点
bool strTrans(QString str); //sharpData字符串解析
void setfaceNorm(float x,float y, float z); //设置面的法向量
void calcNorm(); //计算立方体的法向量
void calcfaceNorm(cubePoint p1,cubePoint p2,cubePoint p3); //计算面的法向量
cubePoint cpArr[8]; //立方体8个点的数组
QList m_positions; //构造体的点的QList
QList m_normals; //构造体的法向量的QList
QList m_indexes; //构造体的索引的QList
QByteArray m_vertexBuffer; //顶点缓存buffer
QByteArray m_indexBuffer; //索引缓存buffer
QString m_sharpData="";
QVector3D boundsMin; //边界最小值
QVector3D boundsMax; //边界最大值
};
#endif // ARMGEOMETRY_H
2.1.2.3 armgeometry.cpp
#include "armgeometry.h"
//! [vertex struct]
struct Vertex {
QVector3D position;
QVector3D normal;
};
//! [vertex struct]
Q_STATIC_ASSERT((sizeof(Vertex)/2)*2 == sizeof(Vertex)); // must be 2-float aligned
//! [constructor]
ArmGeometry::ArmGeometry(QQuick3DObject *parent)
: QQuick3DGeometry(parent)
{
}
//! [constructor]
//! [property]
void ArmGeometry::setSharpData(QString sharpData)
{
if (QString::compare(sharpData,m_sharpData)==0)
return;
m_sharpData = sharpData;
clearCpArr(); //复位构造体
strTrans(sharpData); //把QML的sharpData解析
updateData(); //重绘结构体
update(); //对象更新
emit sharpDataChanged(); //提交构造信号，更新QML
}
//! [property]
// 重绘结构体
void ArmGeometry::updateData()
{
clear(); //QQuick3DGeometry清空
calculateGeometry(); //构建Geometry
//QQuick3DGeometry addAttribute
addAttribute(QQuick3DGeometry::Attribute::PositionSemantic, 0,
QQuick3DGeometry::Attribute::ComponentType::F32Type);
addAttribute(QQuick3DGeometry::Attribute::NormalSemantic, 3 * sizeof(float),
QQuick3DGeometry::Attribute::ComponentType::F32Type);
addAttribute(QQuick3DGeometry::Attribute::IndexSemantic, 0,
QQuick3DGeometry::Attribute::ComponentType::U32Type);
//顶点和索引缓存区构建
const int numVertexes = m_positions.size();
m_vertexBuffer.resize(numVertexes * sizeof(Vertex));
Vertex *vert = reinterpret_cast(m_vertexBuffer.data());
for (int i = 0; i < numVertexes; ++i) {
Vertex &v = vert[i];
v.position = m_positions[i];
v.normal = m_normals[i];
}
setStride(sizeof(Vertex));
setVertexData(m_vertexBuffer);
setPrimitiveType(QQuick3DGeometry::PrimitiveType::Triangles);
setBounds(boundsMin, boundsMax);
m_indexBuffer = QByteArray(reinterpret_cast(m_indexes.data()), m_indexes.size() * sizeof(quint32));
setIndexData(m_indexBuffer);
}
// 重绘Geometry
void ArmGeometry::calculateGeometry()
{
m_positions.clear(); //清空顶点缓存
m_normals.clear(); //清空法向量缓存
m_indexes.clear(); //清空索引缓存
constexpr float maxFloat = std::numeric_limits::max();
boundsMin = QVector3D(maxFloat, maxFloat, maxFloat);
boundsMax = QVector3D(-maxFloat, -maxFloat, -maxFloat);
/*
每个平面建议 单独设置 顶点。顶点不要重用。每个面的各个顶点法向量normal都同值。
（如果顶点重用，那么normal就需要是该顶点所有平面的法向量的平均值，这样比较麻烦）
平面的顶点索引，都按顺时针。
*/
//构建顶点
cubePointAppend(cpArr[0]);
cubePointAppend(cpArr[1]);
cubePointAppend(cpArr[2]);
cubePointAppend(cpArr[3]);
cubePointAppend(cpArr[0]);
cubePointAppend(cpArr[3]);
cubePointAppend(cpArr[4]);
cubePointAppend(cpArr[5]);
cubePointAppend(cpArr[0]);
cubePointAppend(cpArr[5]);
cubePointAppend(cpArr[6]);
cubePointAppend(cpArr[1]);
cubePointAppend(cpArr[1]);
cubePointAppend(cpArr[6]);
cubePointAppend(cpArr[7]);
cubePointAppend(cpArr[2]);
cubePointAppend(cpArr[7]);
cubePointAppend(cpArr[4]);
cubePointAppend(cpArr[3]);
cubePointAppend(cpArr[2]);
cubePointAppend(cpArr[4]);
cubePointAppend(cpArr[7]);
cubePointAppend(cpArr[6]);
cubePointAppend(cpArr[5]);
calcNorm(); //计算立方体法向量
//构建索引缓存
m_indexes<<0;
m_indexes<<1;
m_indexes<<2;
m_indexes<<2;
m_indexes<<3;
m_indexes<<0;
m_indexes<<4;
m_indexes<<5;
m_indexes<<6;
m_indexes<<6;
m_indexes<<7;
m_indexes<<4;
m_indexes<<8;
m_indexes<<9;
m_indexes<<10;
m_indexes<<10;
m_indexes<<11;
m_indexes<<8;
m_indexes<<12;
m_indexes<<13;
m_indexes<<14;
m_indexes<<14;
m_indexes<<15;
m_indexes<<12;
m_indexes<<16;
m_indexes<<17;
m_indexes<<18;
m_indexes<<18;
m_indexes<<19;
m_indexes<<16;
m_indexes<<20;
m_indexes<<21;
m_indexes<<22;
m_indexes<<22;
m_indexes<<23;
m_indexes<<20;
}
// 添加立方体点
void ArmGeometry::cubePointAppend(cubePoint p)
{
m_positions.append({ p.x,p.y,p.z});
}
// 设置立方体点
void ArmGeometry::setCpArr(int id,float x, float y, float z)
{
cpArr[id].x =x;
cpArr[id].y =y;
cpArr[id].z =z;
}
//复位立方体顶点数据
void ArmGeometry::clearCpArr()
{
setCpArr(0,1,1,1);
setCpArr(1,-1,1,1);
setCpArr(2,-1,-1,1);
setCpArr(3,1,-1,1);
setCpArr(4,1,-1,-1);
setCpArr(5,1,1,-1);
setCpArr(6,-1,1,-1);
setCpArr(7,-1,-1,-1);
boundsMin.setX(-1);
boundsMin.setY(-1);
boundsMin.setZ(-1);
boundsMax.setX(1);
boundsMax.setY(1);
boundsMax.setZ(1);
setfaceNorm(0,0,1);
setfaceNorm(1,0,0);
setfaceNorm(0,1,0);
setfaceNorm(-1,0,0);
setfaceNorm(0,-1,0);
setfaceNorm(0,0,-1);
}
//设置面的法向量
void ArmGeometry::setfaceNorm(float x,float y, float z)
{
m_normals.append({x,y,z});
m_normals.append({x,y,z});
m_normals.append({x,y,z});
m_normals.append({x,y,z});
}
//sharpData字符串解析
bool ArmGeometry::strTrans(QString str)
{
bool rp =false;
QStringList list = str.split(",");
if(list.count() != 6)
{
clearCpArr();
return false;
}
float len,wid,hei,cx,cy,cz;
float maxX,maxY,maxZ,minX,minY,minZ;
float topMax;
// 3DView的坐标系：X是横，Y是高，Z是宽
len = QString(list[0]).toFloat();
wid= QString(list[1]).toFloat();
hei= QString(list[2]).toFloat();
cx= QString(list[3]).toFloat();
cy= QString(list[4]).toFloat();
cz= QString(list[5]).toFloat();
boundsMin.setX(-len);
boundsMin.setY(-wid);
boundsMin.setZ(-hei);
boundsMax.setX(len);
boundsMax.setY(wid);
boundsMax.setZ(hei);
setCpArr(0,0.5*len - cx,0.5*wid - cy,0.5*hei- cz);
setCpArr(1,-len*0.5-cx , cpArr[0].y ,cpArr[0].z);
setCpArr(2,cpArr[1].x,-wid*0.5-cy,cpArr[0].z);
setCpArr(3,cpArr[0].x,cpArr[2].y,cpArr[0].z);
setCpArr(4,cpArr[3].x,cpArr[3].y,-hei*0.5-cz);
setCpArr(5,cpArr[0].x,cpArr[0].y,cpArr[4].z);
setCpArr(6,cpArr[1].x,cpArr[1].y,cpArr[4].z);
setCpArr(7,cpArr[2].x,cpArr[2].y,cpArr[4].z);
return rp;
}
//计算立方体的法向量
void ArmGeometry::calcNorm()
{
calcfaceNorm(cpArr[0],cpArr[1],cpArr[2]);
calcfaceNorm(cpArr[0],cpArr[3],cpArr[4]);
calcfaceNorm(cpArr[0],cpArr[5],cpArr[6]);
calcfaceNorm(cpArr[1],cpArr[6],cpArr[7]);
calcfaceNorm(cpArr[7],cpArr[4],cpArr[3]);
calcfaceNorm(cpArr[4],cpArr[7],cpArr[6]);
}
//计算面的法向量
void ArmGeometry::calcfaceNorm(cubePoint p1,cubePoint p2,cubePoint p3)
{
float x,y,z;
x = (p2.y - p1.y)*(p3.z - p1.z) - (p2.z - p1.z)*(p3.y - p1.y);
y = (p2.z - p1.z)*(p3.x - p1.x) - (p3.z - p1.z)*(p2.x - p1.x);
z = (p2.x - p1.x)*(p3.y - p1.y) - (p3.x - p1.x)*(p2.y - p1.y);
setfaceNorm(x,y,z);
}
2.1.2.4 plccomm.h
#ifndef PLCCOMM_H
#define PLCCOMM_H
#include "tcpClinet.h"
#include
#include
#include
#include
#include
#include
//PLC循环查询包的结构体
struct PlcBaseInfo{
int16_t len;
uint16_t cmd;
uint8_t RunningSig;
uint8_t ErrorSig;
uint8_t JogSig;
uint8_t JogModeSig;
float J1;
float J2;
float J3;
float J4;
float J5;
float J6;
float X;
float Y;
float Z;
float A;
float B;
float C;
};
//Plc指令包的结构体
struct PlcCmdMoveAbs
{
uint8_t len;
uint8_t cmd;
uint16_t w;
float X;
float Y;
float Z;
float A;
float B;
float C;
};
class PlcComm:public QObject
{
Q_OBJECT
public:
PlcComm(QObject *parent = nullptr);
bool TryConnect(QString ip,int port); //连接PLC函数
bool IsConnect(); //获取PLC连接状态
QObject *wd;
signals:
void PlcConnectedSig(); //PLC已连接信号
void PlcDisConnectedSig(); //PLC已断开信号
void TcpBreakSig(); //TCP意外断开信号
public slots:
void PlcDisConnectSlot(); //PLC断开连接 槽函数
void PlcConnectSlot(QString ip, int port); //PLC连接 槽函数
void WdClosingSlot(); //窗体关闭 槽函数
void PlcMoveAbsSlot(int tp,qreal x,qreal y,qreal z,qreal a,qreal b,qreal c); //PTP移动 槽函数
void PlcResetSlot(); //PLC复位 槽函数
void HaltMoveSlot(); //轴组暂停 槽函数
void ContinueMoveSlot(); //轴组继续 槽函数
void JogEnableSlot(); //轴组Jog使能 槽函数
void JogDisableSlot(); //轴组Jog失能 槽函数
void JogFwActSlot(int ax); //轴组Jog前进执行 槽函数
void JogFwStopSlot(int ax); //轴组Jog前进停止 槽函数
void JogBwActSlot(int ax); //轴组Jog后退执行 槽函数
void JogBwStopSlot(int ax); //轴组Jog后退停止 槽函数
void JogAxisEnableSlot(); //轴组Jog模式的轴模式使能 槽函数
void JogAxisDisableSlot(); //轴组Jog模式的轴模式失能 槽函数
private slots:
void DecodeReceiSlot(); //接收解析 槽函数
void PlcCircleReadSlot(); //PLC循环查询 槽函数
private:
QTimer* CircleTimer; //循环查询定时器
QTcpSocket* socket=nullptr; //创建socket指针
void DisConnect(); //断开连接 函数
bool SendMsg(QByteArray buf,int size); //TCP发送函数
//接收PLC回复数据，更新QML信号
void PlcDataUpdateSig(uint8_t runlight,uint8_t errorlight,uint8_t joglight,uint8_t jogMode,float J1,float J2,float J3,float J4,float J5,float J6,float L1,float L2,float L3,float L4,float L5,float L6);
char cmdBaseRead[2]={2,1}; //循环查询指令
char cmdReset[2]={2,99}; //复位指令
char cmdHalt[2]={2,80}; //暂停指令
char cmdContinue[2]={2,81}; //继续指令
char cmdJogEnable[3]={3,20,1}; //Jog使能指令
char cmdJogDisable[3]={3,20,0}; //Jog失能指令
char cmdJogFwMove[3]={3,21,0}; //Jog前进指令
char cmdJogFwMoveStop[3]={3,22,0}; //Jog前进停止指令
char cmdJogBwMove[3]={3,23,0}; //Jog后退指令
char cmdJogBwMoveStop[3]={3,24,0}; //Jog后退停止指令
char cmdJogModeSwitch[3]={3,26,1}; //Jog模式切换指令
PlcBaseInfo PlcBase; //PLC循环查询包的结构体实例
};
#endif // PLCCOMM_H
2.1.2.5 plccomm.cpp
#include "plccomm.h"
PlcComm::PlcComm(QObject *parent): QObject(parent)
{
//创建循环查询的定时器
CircleTimer = new QTimer();
CircleTimer->setInterval(200);
connect(CircleTimer, SIGNAL(timeout()), this, SLOT(PlcCircleReadSlot()));
//初始化socket
socket = new QTcpSocket();
}
//窗体关闭 槽函数
void PlcComm::WdClosingSlot()
{
qDebug("窗体 正在关闭");
DisConnect(); //断开TCP连接
}
//PLC连接 槽函数
void PlcComm::PlcConnectSlot(QString ip, int port)
{
//检查是否已连接
if(IsConnect())
{
qDebug("PLC已经连接");
emit PlcConnectedSig(); //PLC已连接信号
}
else
{
qDebug("尝试连接PLC");
if(TryConnect(ip,port))
{
qDebug("PLC连接成功");
emit PlcConnectedSig(); //PLC已连接信号
CircleTimer->start(); //启动定时器循环查询
}
else
{
qDebug("PLC连接失败");
emit PlcDisConnectedSig(); //PLC已断开信号
CircleTimer->stop(); //停止定时器循环查询
}
}
}
//PLC断开连接 槽函数
void PlcComm::PlcDisConnectSlot()
{
//检查是否已连接
if(IsConnect())
{
qDebug("即将断开PLC连接");
DisConnect(); //断开连接 函数
emit PlcDisConnectedSig(); //PLC已断开信号
}
else
{
qDebug("PLC已断开");
emit PlcDisConnectedSig(); //PLC已断开信号
}
CircleTimer->stop(); //停止定时器循环查询
}
//PLC循环查询 槽函数
void PlcComm::PlcCircleReadSlot()
{
//检查是否已连接
if(IsConnect())
{
QByteArray buf(cmdBaseRead); //cmdBaseRead转QByteArray
SendMsg(buf,2); //TCP发送函数
}
else
{
qDebug("提示:PLC已断开");
CircleTimer->stop(); //停止定时器循环查询
emit TcpBreakSig(); //发出TCP意外断开信号
DisConnect(); //断开连接 函数
}
}
//连接PLC函数
bool PlcComm::TryConnect(QString ip,int port)
{
bool isconnect = false;
if(socket!=nullptr)
{
socket->abort();
socket->setSocketOption(QAbstractSocket::LowDelayOption, 1);
socket->connectToHost(ip, port);
isconnect = socket->waitForConnected(500);//等待直到连接成功
//如果连接成功
if (isconnect)
{
qDebug("TCP已连接");
connect(socket, &QTcpSocket::readyRead, this, &PlcComm::DecodeReceiSlot); //绑定readyRead和 接收解析 槽函数
}
else
{
qDebug("TCP连接失败");
}
}
return isconnect;
}
//TCP发送函数
bool PlcComm::SendMsg(QByteArray buf,int size)
{
bool iswrite = false;
if (IsConnect())
{
socket->write(buf,size);
//判断是否写入成功
iswrite= socket->waitForBytesWritten();
if(!iswrite)
{
DisConnect(); //断开连接 函数
emit TcpBreakSig(); //发送TCP意外断开信号
}
}
else
{
emit TcpBreakSig(); //发送TCP意外断开信号
}
return iswrite;
}
//接收解析 槽函数
void PlcComm::DecodeReceiSlot()
{
//获取socket的字节
QByteArray buf = socket->readAll();
int buflen = buf.length();
int r_addr =0;
int cmdlen =0;
QByteArray tmp;
PlcBaseInfo* pbi;
//TCP将粘包拆包
while(r_addr < buflen)
{
cmdlen = (int16_t)buf[r_addr];
tmp.clear();
if(cmdlen == 0)
{
break;
}
else if((cmdlen+r_addr)>buflen)
{
break;
}
tmp.append(buf+r_addr,cmdlen);
pbi= (PlcBaseInfo*)tmp.data();
if(pbi->cmd == 1)
{
PlcDataUpdateSig(pbi->RunningSig,pbi->ErrorSig,pbi->JogSig,pbi->JogModeSig, pbi->J1,pbi->J2,pbi->J3,pbi->J4,pbi->J5,pbi->J6,pbi->X,pbi->Y,pbi->Z,pbi->A,pbi->B,pbi->C);
}
r_addr = r_addr + cmdlen;
}
}
//获取TCP连接状态
bool PlcComm::IsConnect()
{
if(socket==nullptr)
{
return false;
}
if(socket->state() == 3) //state == 3 , connected
{
return true;
}
else
{
return false;
}
}
//断开连接 函数
void PlcComm::DisConnect()
{
socket->disconnectFromHost();
qDebug("TCP已断开");
}
//接收PLC回复数据，更新QML信号
void PlcComm::PlcDataUpdateSig(uint8_t runlight,uint8_t errorlight,uint8_t joglight,uint8_t jogMode,float J1,float J2,float J3,float J4,float J5,float J6,float L1,float L2,float L3,float L4,float L5,float L6)
{
//通过Json格式，把多参数发送到QML
QVariant ret;//用于保存qml中function的返回值
QString info;
QJsonObject jsonObject;
jsonObject.insert("rlight", runlight);
jsonObject.insert("elight", errorlight);
jsonObject.insert("jlight", joglight);
jsonObject.insert("jmode", jogMode);
jsonObject.insert("j1", J1);
jsonObject.insert("j2", J2);
jsonObject.insert("j3", J3);
jsonObject.insert("j4", J4);
jsonObject.insert("j5", J5);
jsonObject.insert("j6", J6);
jsonObject.insert("l1", L1);
jsonObject.insert("l2", L2);
jsonObject.insert("l3", L3);
jsonObject.insert("l4", L4);
jsonObject.insert("l5", L5);
jsonObject.insert("l6", L6);
// 使用QJsonDocument设置该json对象
QJsonDocument jsonDoc;
jsonDoc.setObject(jsonObject);
info = jsonDoc.toJson(QJsonDocument::Compact);
QMetaObject::invokeMethod(wd, "setArmJoint", Q_RETURN_ARG(QVariant, ret),
Q_ARG(QVariant, info)
); //Q_RETURN_ARG指定返回值，Q_ARG指定输入的参数
}
//PTP移动 槽函数
void PlcComm::PlcMoveAbsSlot(int tp,qreal x,qreal y,qreal z,qreal a,qreal b,qreal c)
{
PlcCmdMoveAbs w;
w.len = sizeof(w);
w.cmd = (uchar)tp;
w.w = 0;
w.X = (float)x;
w.Y = (float)y;
w.Z = (float)z;
w.A = (float)a;
w.B = (float)b;
w.C = (float)c;
QByteArray array;
array.append((char*)&w,w.len );
SendMsg(array,w.len);
}
//PLC复位 槽函数
void PlcComm::PlcResetSlot()
{
QByteArray buf(cmdReset);
SendMsg(buf,2);
}
//轴组暂停 槽函数
void PlcComm::HaltMoveSlot()
{
QByteArray buf(cmdHalt);
SendMsg(buf,2);
}
//轴组继续 槽函数
void PlcComm::ContinueMoveSlot()
{
QByteArray buf(cmdContinue);
SendMsg(buf,2);
}
//轴组Jog使能 槽函数
void PlcComm::JogEnableSlot()
{
QByteArray buf(cmdJogEnable);
SendMsg(buf,3);
}
//轴组Jog失能 槽函数
void PlcComm::JogDisableSlot()
{
QByteArray buf(cmdJogDisable);
SendMsg(buf,3);
}
//轴组Jog前进执行 槽函数
void PlcComm::JogFwActSlot(int ax)
{
cmdJogFwMove[2] = (uchar)ax;
QByteArray buf(cmdJogFwMove);
SendMsg(buf,3);
}
//轴组Jog前进停止 槽函数
void PlcComm::JogFwStopSlot(int ax)
{
cmdJogFwMoveStop[2] = (uchar)ax;
QByteArray buf(cmdJogFwMoveStop);
SendMsg(buf,3);
}
//轴组Jog后退执行 槽函数
void PlcComm::JogBwActSlot(int ax)
{
cmdJogBwMove[2] = (uchar)ax;
QByteArray buf(cmdJogBwMove);
SendMsg(buf,3);
}
//轴组Jog后退停止 槽函数
void PlcComm::JogBwStopSlot(int ax)
{
cmdJogBwMoveStop[2] = (uchar)ax;
QByteArray buf(cmdJogBwMoveStop);
SendMsg(buf,3);
}
//轴组Jog模式的轴模式使能 槽函数
void PlcComm::JogAxisEnableSlot()
{
cmdJogModeSwitch[2] = 1;
QByteArray buf(cmdJogModeSwitch);
SendMsg(buf,3);
}
//轴组Jog模式的轴模式失能 槽函数
void PlcComm::JogAxisDisableSlot()
{
cmdJogModeSwitch[2] = 0;
QByteArray buf(cmdJogModeSwitch);
SendMsg(buf,3);
}
2.1.2.6 main.cpp
#include
#include
#include
#include "armobj.h"
#include "plccomm.h"
QThread* subThread ;
int main(int argc, char *argv[])
{
QGuiApplication app(argc, argv);
QQmlApplicationEngine engine;
const QUrl url(u"qrc:/robotArm1/main.qml"_qs);
QObject::connect(&engine, &QQmlApplicationEngine::objectCreated,
&app, [url](QObject *obj, const QUrl &objUrl) {
if (!obj && url == objUrl)
QCoreApplication::exit(-1);
}, Qt::QueuedConnection);
engine.load(url);
PlcComm plc;
QObject *wd = engine.rootObjects().first();
plc.wd = wd;
QObject::connect(wd, SIGNAL(plcTryConnectSig(QString,int)), &plc, SLOT(PlcConnectSlot(QString,int)),Qt::DirectConnection);
QObject::connect(wd, SIGNAL(plcDisconnectSignal()), &plc, SLOT(PlcDisConnectSlot()),Qt::DirectConnection);
QObject::connect(wd, SIGNAL(wdOnClosingSignal()), &plc, SLOT(WdClosingSlot()),Qt::DirectConnection);
QObject::connect(wd, SIGNAL(moveAbsAct(int,qreal,qreal,qreal,qreal,qreal,qreal)), &plc, SLOT(PlcMoveAbsSlot(int,qreal,qreal,qreal,qreal,qreal,qreal)),Qt::DirectConnection);
QObject::connect(wd, SIGNAL(resetError()), &plc, SLOT(PlcResetSlot()),Qt::DirectConnection);
QObject::connect(wd, SIGNAL(haltMoving()), &plc, SLOT(HaltMoveSlot()),Qt::DirectConnection);
QObject::connect(wd, SIGNAL(continueMoving()), &plc, SLOT(ContinueMoveSlot()),Qt::DirectConnection);
QObject::connect(wd, SIGNAL(jogEnable()), &plc, SLOT(JogEnableSlot()),Qt::DirectConnection);
QObject::connect(wd, SIGNAL(jogDisable()), &plc, SLOT(JogDisableSlot()),Qt::DirectConnection);
QObject::connect(wd, SIGNAL(jogFwAct(int)), &plc, SLOT(JogFwActSlot(int)),Qt::DirectConnection);
QObject::connect(wd, SIGNAL(jogFwStop(int)), &plc, SLOT(JogFwStopSlot(int)),Qt::DirectConnection);
QObject::connect(wd, SIGNAL(jogBwAct(int)), &plc, SLOT(JogBwActSlot(int)),Qt::DirectConnection);
QObject::connect(wd, SIGNAL(jogBwStop(int)), &plc, SLOT(JogBwStopSlot(int)),Qt::DirectConnection);
QObject::connect(wd, SIGNAL(jogAxisEnable()), &plc, SLOT(JogAxisEnableSlot()),Qt::DirectConnection);
QObject::connect(wd, SIGNAL(jogAxisDisable()), &plc, SLOT(JogAxisDisableSlot()),Qt::DirectConnection);
QObject::connect(&plc, SIGNAL(PlcConnectedSig()), wd, SLOT(connectLightOn()));
QObject::connect(&plc, SIGNAL(PlcDisConnectedSig()), wd, SLOT(connectLightOff()));
QObject::connect(&plc, SIGNAL(TcpBreakSig()), wd, SLOT(connectLightOff()));
return app.exec();
}
2.1.2.7 main.qml
import QtQuick 2.15
import QtQuick3D
import QtQuick.Layouts 1.0
import QtQuick.Controls 2.5
import RobotArm
ApplicationWindow {
id: window
visible: true
width: 1366
height: 768
title: qsTr("AlongWu RobotArm Example")
//QML signal
signal plcTryConnectSig(string ip,int port)
signal plcDisconnectSignal()
signal wdOnClosingSignal()
signal moveAbsAct(int tp,real x,real y,real z,real a,real b,real c)
signal resetError();
signal haltMoving();
signal continueMoving();
signal jogEnable();
signal jogDisable();
signal jogAxisDisable();
signal jogAxisEnable();
signal jogFwAct(int ax);
signal jogFwStop(int ax);
signal jogBwAct(int ax);
signal jogBwStop(int ax);
//QML Functions
function runningLightOn()
{
runningLightColor ="red";
}
function runningLightOff()
{
runningLightColor ="white";
}
function connectLightOn()
{
connectToggle.checked=true;
connectToggle.text ="控制器已联机";
}
function connectLightOff()
{
connectToggle.checked=false;
connectToggle.text ="控制器已断开";
}
function errorLigthOn()
{
errorLightColor="red";
}
function errorLigthOff()
{
errorLightColor="white";
}
function jogLigthOn()
{
jogToggle.checked=true;
jogToggle.text ="JOG模式已启用";
}
function jogLigthOff()
{
jogToggle.checked=false;
jogToggle.text ="JOG模式已禁用";
}
function jogModeLigthOn()
{
jogModeToggle.checked=true;
jogModeToggle.text ="轴坐标模式";
}
function jogModeLigthOff()
{
jogModeToggle.checked=false;
jogModeToggle.text ="世界坐标模式";
}
function connectSwitch()
{
if(connectToggle.checked)
{
plcDisconnectSignal();
}
else
{
plcTryConnectSig(ip_tf.text,parseInt(port_tf.text));
}
}
function jogSwitch()
{
if(jogToggle.checked)
{
jogDisable();
}
else
{
jogEnable();
}
}
function jogModeSwitch()
{
if(jogModeToggle.checked)
{
jogAxisDisable();
}
else
{
jogAxisEnable();
}
}
function setArmJoint(info)
{
var obj = JSON.parse(info);
j1=(obj.j1).toFixed(2);
j2=(obj.j2).toFixed(2);
j3=(obj.j3).toFixed(2);
j4=(obj.j4).toFixed(2);
j5=(obj.j5).toFixed(2);
j6=(obj.j6).toFixed(2);
loc_x=(obj.l1).toFixed(2);
loc_y=(obj.l2).toFixed(2);
loc_z=(obj.l3).toFixed(2);
loc_a=(obj.l4).toFixed(2);
loc_b=(obj.l5).toFixed(2);
loc_c=(obj.l6).toFixed(2);
end_x = loc_x/100;
end_y = loc_y/100;
end_z = loc_z/100;
end_a = loc_a/100;
end_b = loc_b/100;
end_c = loc_c/100;
if(obj.rlight == 1)
{
runningLightOn();
}
else
{
runningLightOff();
}
if(obj.elight == 1)
{
errorLigthOn();
}
else
{
errorLigthOff();
}
if(obj.jlight == 1)
{
jogLigthOn();
}
else
{
jogLigthOff();
}
if(obj.jmode == 1)
{
jogModeLigthOn();
}
else
{
jogModeLigthOff();
}
}
onClosing:
{
wdOnClosingSignal(); //发送标题栏窗体关闭信号
}
//初始化DH表格
property real d1:1-0.25;
property real a1:1-0.25;
property real a2:4-0.25;
property real a3:1-0.25;
property real d3:0.7-0.25;
property real d4:3.5-0.25;
property real d6:0.5;
property real v_z;
property real j1:0;
property real j2:0;
property real j3:0;
property real j4:0;
property real j5:0;
property real j6:0;
property real end_x:3.5;
property real end_y:-0.7;
property real end_z:9.50;
property real end_a:0;
property real end_b:0;
property real end_c:0;
property real loc_x:0;
property real loc_y:0;
property real loc_z:0;
property real loc_a:0;
property real loc_b:0;
property real loc_c:0;
property string runningLightColor:"white";
property string errorLightColor:"white";
View3D {
id: view
anchors.fill: parent
environment: SceneEnvironment {
clearColor:"LightGray"
backgroundMode: SceneEnvironment.Color
}
DirectionalLight {
eulerRotation.x: -10
NumberAnimation on eulerRotation.y {
from: 0
to: 360
duration: 17000
loops: Animation.Infinite
}
ambientColor: Qt.rgba(0.3, 0.3, 0.3, 1.0)
}
PointLight {
position: Qt.vector3d(-15, 10, 15)
}
PerspectiveCamera {
id: camera
position.z: 20.0*farSlider.value
position.y: 5.0*farSlider.value
eulerRotation.x: -10
clipNear: 1.0
clipFar: 40
}
PrincipledMaterial {
id: material
baseColor: "#af4f1f"
roughness: 0.3
specularAmount: 0.6
}
PrincipledMaterial {
id: material2
baseColor: "#ffff00"
roughness: 0.3
specularAmount: 0.6
}
//! [target]
//! [model]
Model {
id:armModel
eulerRotation.x: -90
geometry: deskPcs
materials: material
eulerRotation.z : modelSlider.value
Model {
z:d1
geometry: rootPcs
materials: material2
eulerRotation.z :j1
Model {
z:0.25
geometry: hand1
materials: material
Model {
z:0
x:a1+0.25
geometry: hand2
materials: material2
eulerRotation.y: -j2
Model {
z:a2+0.25
x:0
y:0
geometry: hand3
materials: material
Model {
y:-d3-0.25
z:0
x:0
eulerRotation.y: -j3
geometry: hand4
materials: material2
Model {
y:0
z:a3+0.25
eulerRotation.x: j4
geometry: hand5
materials: material
Model {
x:d4
eulerRotation.y: j5
geometry: hand6
materials: material2
Model {
eulerRotation.z: 90
x:1
scale: Qt.vector3d(0.005, 0.015,0.005)
source: "#Cylinder"
materials: [ DefaultMaterial {
diffuseColor: "red"
}
]
}
}
}
}
}
}
}
}
}
ArmGeometry
{
id:deskPcs
sharpData:"3,3,0.2,0,0,0.1"
}
ArmGeometry
{
id:rootPcs
sharpData:"1,1,"+d1+",0,0,"+d1*0.5
}
ArmGeometry
{
id:hand1
sharpData:a1+",0.5,0.5,"+(-0.5*a1)+",0,0"
}
ArmGeometry
{
id:hand2
sharpData:"0.5,0.5,"+a2+",0,0,"+(-a2*0.5)
}
ArmGeometry
{
id:hand3
sharpData:"0.5,"+d3+",0.5,0,"+(0.5*d3)+",0"
}
ArmGeometry
{
id:hand4
sharpData:"0.5,0.5,"+a3+",0,0,"+(-0.5*a3)
}
ArmGeometry
{
id:hand5
sharpData:+d4+",0.5,0.5,"+(-0.5*d4)+",0,0"
}
ArmGeometry
{
id:hand6
sharpData:d6+",0.5,0.5,"+(-0.5*d6)+",0,0"
}
ArmGeometry
{
id:hand7
sharpData:"0.5,0.5,0.5,0,0,0"
}
//! [model]
}
Frame {
width:500
background: Rectangle {
color: "#c0c0c0"
border.color: "#202020"
}
anchors.top: parent.top
anchors.left: parent.left
anchors.margins: 10
Column {
spacing: 10
id: settingsArea
Text {
Layout.alignment: Qt.AlignHCenter
font.bold: true
text: "6关节机械臂操作板"
}
RowLayout{
height: 20
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "IP地址："
}
Rectangle {
width: 120
height: 20
color: "white"
TextInput
{
width:parent.width-20
height: parent.height-5
anchors.centerIn: parent
id:ip_tf
text:"192.168.1.103"
horizontalAlignment:Qt.AlignHCenter
validator: RegularExpressionValidator { regularExpression: /([0,1]?\d{1,2}|2([0-4][0-9]|5[0-5]))(\.([0,1]?\d{1,2}|2([0-4][0-9]|5[0-5]))){3}/ }
}
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "Port："
}
Rectangle {
width: 80
height: 20
color: "white"
TextInput
{
width:parent.width-10
height: parent.height-5
anchors.centerIn: parent
horizontalAlignment:Qt.AlignHCenter
id:port_tf
text:"10010"
validator: RegularExpressionValidator { regularExpression: /\b\d{0,4}\b|\b[1-5]\d{0,4}\b|\b6[0,4]\d{0,3}\b|\b65[0-4]\d{0,2}\b|\b655[0-2]\d\b|\b6553[0-5]\b/ }
}
}
}
Row{
width: 400
Toggle
{
id: connectToggle
text: qsTr("控制器已断开")
Button{
opacity: 0
anchors.fill: parent
onClicked:
{
connectSwitch();
}
}
}
}
RowLayout{
width: 350
height: 25
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "运行指示灯"
}
Rectangle {
id: runingLight
width: 20
height: 20
radius: 100
color: runningLightColor
border.width: 0.5
border.color: "black"
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "故障指示灯"
}
Rectangle {
id: errorLigth
width: 20
height: 20
radius: 100
color: errorLightColor
border.width: 0.5
border.color: "black"
}
Button {
id: resetButton
Layout.preferredWidth: 80
text: "复位"
onClicked:
{
resetError();
}
}
Button {
id: haltButton
Layout.preferredWidth: 80
text: "暂停"
onClicked:
{
haltMoving();
}
}
Button {
id: continueButton
Layout.preferredWidth: 80
text: "继续"
onClicked:
{
continueMoving();
}
}
}
GridLayout {
columns: 6
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "目标点 J1 / 坐标X："
}
Rectangle {
width: 70
height: 20
color: "white"
TextInput
{
width:parent.width-10
height: parent.height-3
anchors.centerIn: parent
horizontalAlignment:Qt.AlignHCenter
id:point_x_tf
text:"0"
validator: RegularExpressionValidator { regularExpression: /(^(-)?\d{0,4})|(^(-)?\d{0,4}\.\d{0,3})/ }
}
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "当前 坐标X："
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: loc_x
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "当前 J1："
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: j1
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "目标点 J2 / 坐标Y："
}
Rectangle {
width: 70
height: 20
color: "white"
TextInput
{
width:parent.width-10
height: parent.height-3
anchors.centerIn: parent
horizontalAlignment:Qt.AlignHCenter
id:point_y_tf
text:"0"
validator: RegularExpressionValidator { regularExpression: /(^(-)?\d{0,4})|(^(-)?\d{0,4}\.\d{0,3})/ }
}
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "当前 坐标Y："
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: loc_y
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "当前 J2："
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: j2
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "目标点 J3 / 坐标Z："
}
Rectangle {
width: 70
height: 20
color: "white"
TextInput
{
width:parent.width-10
height: parent.height-3
anchors.centerIn: parent
horizontalAlignment:Qt.AlignHCenter
id:point_z_tf
text:"0"
validator: RegularExpressionValidator { regularExpression: /(^(-)?\d{0,4})|(^(-)?\d{0,4}\.\d{0,3})/ }
}
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "当前 坐标Z："
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: loc_z
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "当前 J3："
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: j3
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "目标点 J4 / 坐标A："
}
Rectangle {
width: 70
height: 20
color: "white"
TextInput
{
width:parent.width-10
height: parent.height-3
anchors.centerIn: parent
horizontalAlignment:Qt.AlignHCenter
id:point_a_tf
text:"0"
validator: RegularExpressionValidator { regularExpression: /(^(-)?\d{0,4})|(^(-)?\d{0,4}\.\d{0,3})/ }
}
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "当前 坐标A："
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: loc_a
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "当前 J4："
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: j4
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "目标点 J5 / 坐标B："
}
Rectangle {
width: 70
height: 20
color: "white"
TextInput
{
width:parent.width-10
height: parent.height-3
anchors.centerIn: parent
horizontalAlignment:Qt.AlignHCenter
id:point_b_tf
text:"0"
validator: RegularExpressionValidator { regularExpression: /(^(-)?\d{0,4})|(^(-)?\d{0,4}\.\d{0,3})/ }
}
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "当前 坐标B："
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: loc_b
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "当前 J5："
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: j5
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "目标点 J6 / 坐标C："
}
Rectangle {
width: 70
height: 20
color: "white"
TextInput
{
width:parent.width-10
height: parent.height-3
anchors.centerIn: parent
horizontalAlignment:Qt.AlignHCenter
id:point_c_tf
text:"0"
validator: RegularExpressionValidator { regularExpression: /(^(-)?\d{0,4})|(^(-)?\d{0,4}\.\d{0,3})/ }
}
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "当前 坐标C："
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: loc_c
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: "当前 J6："
}
Text {
Layout.alignment: Qt.AlignHCenter
width: 60
height: 20
text: j6
}
}
RowLayout{
width: 350
height: 25
Button {
id: ptpButton
Layout.preferredWidth: 120
text: "PTP移动"
onClicked: {
moveAbsAct(2,parseFloat(point_x_tf.text),parseFloat(point_y_tf.text),parseFloat(point_z_tf.text),parseFloat(point_a_tf.text),parseFloat(point_b_tf.text),parseFloat(point_c_tf.text))
}
}
Button {
id: lineButton
Layout.preferredWidth: 120
text: "直线移动"
onClicked: {
moveAbsAct(3,parseFloat(point_x_tf.text),parseFloat(point_y_tf.text),parseFloat(point_z_tf.text),parseFloat(point_a_tf.text),parseFloat(point_b_tf.text),parseFloat(point_c_tf.text))
}
}
Button {
id: axisButton
Layout.preferredWidth: 120
text: "关节移动"
onClicked: {
moveAbsAct(4,parseFloat(point_x_tf.text),parseFloat(point_y_tf.text),parseFloat(point_z_tf.text),parseFloat(point_a_tf.text),parseFloat(point_b_tf.text),parseFloat(point_c_tf.text))
}
}
}
Row{
width: 400
height: 25
Toggle
{
id: jogToggle
text: qsTr("JOG模式已禁用")
Button{
opacity: 0
anchors.fill: parent
onClicked:
{
jogSwitch();
}
}
}
Toggle
{
id: jogModeToggle
text: qsTr("世界坐标模式")
Button{
opacity: 0
anchors.fill: parent
onClicked:
{
jogModeSwitch();
}
}
}
}
RowLayout{
width: 350
height: 25
Button {
Layout.preferredWidth: 50
text: "X+"
onPressed:
{
jogFwAct(1);
}
onReleased:
{
jogFwStop(1);
}
}
Button {
Layout.preferredWidth: 50
text: "X-"
onPressed:
{
jogBwAct(1);
}
onReleased:
{
jogBwStop(1);
}
}
Button {
Layout.preferredWidth: 50
text: "Y+"
onPressed:
{
jogFwAct(2);
}
onReleased:
{
jogFwStop(2);
}
}
Button {
Layout.preferredWidth: 50
text: "Y-"
onPressed:
{
jogBwAct(2);
}
onReleased:
{
jogBwStop(2);
}
}
Button {
Layout.preferredWidth: 50
text: "Z+"
onPressed:
{
jogFwAct(3);
}
onReleased:
{
jogFwStop(3);
}
}
Button {
Layout.preferredWidth: 50
text: "Z-"
onPressed:
{
jogBwAct(3);
}
onReleased:
{
jogBwStop(3);
}
}
}
RowLayout{
width: 350
height: 25
Button {
Layout.preferredWidth: 50
text: "A+"
onPressed:
{
jogFwAct(4);
}
onReleased:
{
jogFwStop(4);
}
}
Button {
Layout.preferredWidth: 50
text: "A-"
onPressed:
{
jogBwAct(4);
}
onReleased:
{
jogBwStop(4);
}
}
Button {
Layout.preferredWidth: 50
text: "B+"
onPressed:
{
jogFwAct(5);
}
onReleased:
{
jogFwStop(5);
}
}
Button {
Layout.preferredWidth: 50
text: "B-"
onPressed:
{
jogBwAct(5);
}
onReleased:
{
jogBwStop(5);
}
}
Button {
Layout.preferredWidth: 50
text: "C+"
onPressed:
{
jogFwAct(6);
}
onReleased:
{
jogFwStop(6);
}
}
Button {
Layout.preferredWidth: 50
text: "C-"
onPressed:
{
jogBwAct(6);
}
onReleased:
{
jogBwStop(6);
}
}
}
Row{
width: 350
height: 25
Text {
width: 50
text: "视角:"
}
Slider {
id: modelSlider
width:300
from: 0
to: 360
value: 0
}
}
Row{
width: 350
height: 25
Text {
width: 50
text: "远近:"
}
Slider {
id: farSlider
width:300
from: 0
to: 3
value: 1
}
}
}
}
}
2.2CodeSys
2.2.1 程序组织
2.2.2 程序代码
2.2.2.1 KinGroup
2.2.2.2 GroupFb_RPG
//作者： AlongWu
PROGRAM GroupFB_RPG
VAR
END_VAR
//轴组故障读取功能块
GVL.GroupReadError(
AxisGroup:= KinGroup,
Enable:= TRUE,
Valid=> ,
Busy=> ,
Error=> ,
ErrorID=> ,
GroupErrorID=> );
//轴组故障标志
IF GVL.GroupReadError.GroupErrorID > 0 OR GVL.GroupJog2A.Error OR GVL.GroupJog2B.Error THEN
GVL.B_PlanError:=TRUE;
ELSE
GVL.B_PlanError:=FALSE;
END_IF
//轴组使能功能块
GVL.GroupPower(
AxisGroup:= KinGroup,
Enable:= GVL.B_GpPower,
bRegulatorOn:= TRUE,
bDriveStart:= TRUE,
Status=> ,
Busy=> ,
Error=> ,
ErrorID=> );
//轴组运动学使能功能块
GVL.GroupEnable(
AxisGroup:=KinGroup,
Execute:=GVL.B_GpEnable
);
//轴组复位功能块
GVL.GroupReset(
AxisGroup:= KinGroup,
Execute:= GVL.B_ResetBt,
Done=> ,
Busy=> ,
Error=> ,
ErrorID=> );
IF GVL.GroupReset.Done THEN
GVL.B_ResetBt:=FALSE;
END_IF
//轴组点对点绝对式移动功能块
GVL.GroupMoveAbs(
AxisGroup:= KinGroup,
Execute:= GVL.B_GroupMoveAbsEnble,
Position:= GVL.GroupMoveAbsTarget_SMC_POS_REF ,
MovementType:= ,
CoordSystem:=SMC_COORD_SYSTEM.WCS,
BufferMode:= MC_BUFFER_MODE.Aborting,
TransitionMode:= ,
TransitionParameter:= ,
VelFactor:= 1,
AccFactor:= 1,
JerkFactor:= 1,
Done=> ,
Busy=> ,
Active=> ,
CommandAborted=> ,
CommandAccepted=> ,
Error=> ,
ErrorID=> ,
MovementId=> );
//轴组轴角度绝对式移动功能块
GVL.JointMoveAbs(
AxisGroup:= KinGroup,
Execute:= GVL.B_JointMoveAbsEnble,
Position:= GVL.GroupMoveAbsTarget_SMC_POS_REF ,
MovementType:= ,
CoordSystem:=SMC_COORD_SYSTEM.ACS,
BufferMode:= MC_BUFFER_MODE.Aborting,
TransitionMode:= ,
TransitionParameter:= ,
VelFactor:= 1,
AccFactor:= 1,
JerkFactor:= 1,
Done=> ,
Busy=> ,
Active=> ,
CommandAborted=> ,
CommandAccepted=> ,
Error=> ,
ErrorID=> ,
MovementId=> );
//轴组直线插补功能块
GVL.MoveLinearAbsolute(
AxisGroup:= KinGroup,
Execute:= GVL.B_GroupLineAbsEnble,
Position:=GVL.GroupMoveAbsTarget_SMC_POS_REF ,
Velocity:= 50,
Acceleration:= 50,
Deceleration:= 50,
Jerk:= 50,
CoordSystem:= SMC_COORD_SYSTEM.WCS,
BufferMode:= MC_BUFFER_MODE.Aborting,
TransitionMode:= ,
TransitionParameter:= ,
OrientationMode:= ,
VelFactor:= ,
AccFactor:= ,
JerkFactor:= ,
Done=> ,
Busy=> ,
Active=> ,
CommandAborted=> ,
CommandAccepted=> ,
Error=> ,
ErrorID=> ,
MovementId=> );
//轴组JOG wcs
GVL.GroupJog2A(
AxisGroup:= KinGroup,
Enable:= GVL.GroupJogEnable AND NOT (GVL.GroupJogMode),
Forward:= GVL.JogForwardGroup,
Backward:= GVL.JogBackwardGroup,
MaxLinearDistance:= 500,
MaxAngularDistance:= 360,
Velocity:= 120,
Acceleration:= 200,
Deceleration:= 200,
Jerk:= 120,
VelFactor:= 1,
AccFactor:= 1,
JerkFactor:= 1,
CoordSystem:= SMC_COORD_SYSTEM.WCS,
ABC_as_ACS:= FALSE,
Active=> ,
Busy=> ,
Error=> ,
ErrorID=> ,
CurrentPosition=> );
//轴组JOG acs
GVL.GroupJog2B(
AxisGroup:= KinGroup,
Enable:= GVL.GroupJogEnable AND GVL.GroupJogMode,
Forward:= GVL.JogForwardGroup,
Backward:= GVL.JogBackwardGroup,
MaxLinearDistance:= 100,
MaxAngularDistance:= 360,
Velocity:= 80,
Acceleration:= 100,
Deceleration:= 100,
Jerk:= 80,
VelFactor:= 1,
AccFactor:= 1,
JerkFactor:= 1,
CoordSystem:= SMC_COORD_SYSTEM.ACS,
ABC_as_ACS:= FALSE,
Active=> ,
Busy=> ,
Error=> ,
ErrorID=> ,
CurrentPosition=> );
//轴组暂停功能块
GVL.GroupHalt(
AxisGroup:= KinGroup,
Execute:= GVL.B_GroupHalt,
Deceleration:= 50,
Jerk:= 50,
AccFactor:= 1,
JerkFactor:= 1,
Done=> ,
Busy=> ,
Active=> ,
CommandAborted=> ,
CommandAccepted=> ,
Error=> ,
ErrorID=> ,
MovementId=> );
//轴组保存continu data
GVL.SaveContiueData(
AxisGroup:= KinGroup,
continueData:= GVL.ContiueDataStru,
Execute:= GVL.GroupHalt.Done,
Busy=> ,
Error=> ,
ErrorID=> ,
Done=> );
//轴组继续运行
GVL.GroupContinue(
AxisGroup:= KinGroup,
continueData:= GVL.ContiueDataStru,
Execute:=GVL.B_GroupContinue ,
Done=> ,
CommandAborted=> ,
Busy=> ,
Error=> ,
ErrorID=> );
IF GVL.GroupContinue.Done THEN
GVL.B_GroupContinue :=FALSE;
END_IF2.2.2.3 GroupLine_RPG
//作者： AlongWu
PROGRAM GroupLine_RPG
VAR
i :INT;
END_VAR
//轴组直线插补子程序
CASE GVL.I_LineM_Step OF
0:
GVL.B_GroupLineAbsEnble:=TRUE;
GVL.I_LineM_Step:=GVL.I_LineM_Step + 1;
1:
IF GVL.MoveLinearAbsolute.Done THEN
GVL.B_GroupLineAbsEnble:=FALSE;
GVL.I_LineM_Step:=GVL.I_LineM_Step + 1;
END_IF
IF GVL.B_PlanError THEN
GVL.I_LineM_Step:=10;
i := 0;
GVL.B_GroupLineAbsEnble:=FALSE;
GVL.MotionType := MotionType_Enum.Wait;
END_IF
ELSE
GVL.MotionType := MotionType_Enum.Wait;
END_CASE
2.2.2.4 PLC_PRG
//作者： AlongWu
PROGRAM PLC_PRG
VAR
Car_pos :SMC_GroupReadSetPosition; //读取当前终端点坐标系在世界坐标系的值的功能块 实例
Axis_pos :SMC_GroupReadSetPosition; //读取当前终端点坐标系在轴坐标系的值的功能块 实例
END_VAR
//使能 轴组运动学功能块
IF GVL.B_GpEnable = FALSE THEN
IF GVL.GroupPower.Status THEN
GVL.B_GpEnable:=TRUE;
END_IF
END_IF
// 读取当前目标坐标系
Car_pos
(
AxisGroup:=KinGroup,
CoordSystem:=SM3_Robotics.SMC_COORD_SYSTEM.WCS,
Enable:=TRUE
);
Axis_pos
(
AxisGroup:=KinGroup,
CoordSystem:=SM3_Robotics.SMC_COORD_SYSTEM.ACS,
Enable:=TRUE
);
//轴组移动状态
IF GVL.B_MotionAct THEN
GVL.BaseRely.RunningSig := 1;
ELSE
GVL.BaseRely.RunningSig := 0;
END_IF
//轴组故障信号
IF GVL.B_PlanError THEN
GVL.BaseRely.ErrorSig := 1;
ELSE
GVL.BaseRely.ErrorSig := 0;
END_IF
//轴组Jog使能标志
IF GVL.GroupJogEnable THEN
GVL.BaseRely.JogSig :=1;
ELSE
GVL.BaseRely.JogSig :=0;
END_IF
//轴组Jog模式标志
IF GVL.GroupJogMode THEN
GVL.BaseRely.JogModeSig :=1;
ELSE
GVL.BaseRely.JogModeSig :=0;
END_IF
//赋值tcp报文
GVL.BaseRely.J1 := LREAL_TO_REAL(Axis_pos.Position.a.a0);
GVL.BaseRely.J2 := LREAL_TO_REAL(Axis_pos.Position.a.a1);
GVL.BaseRely.J3 := LREAL_TO_REAL(Axis_pos.Position.a.a2);
GVL.BaseRely.J4 := LREAL_TO_REAL(Axis_pos.Position.a.a3);
GVL.BaseRely.J5 := LREAL_TO_REAL(Axis_pos.Position.a.a4);
GVL.BaseRely.J6 := LREAL_TO_REAL(Axis_pos.Position.a.a5);
GVL.BaseRely.X := LREAL_TO_REAL(Car_pos.Position.c.X);
GVL.BaseRely.Y := LREAL_TO_REAL(Car_pos.Position.c.Y);
GVL.BaseRely.Z := LREAL_TO_REAL(Car_pos.Position.c.Z);
GVL.BaseRely.A := LREAL_TO_REAL(Car_pos.Position.c.A);
GVL.BaseRely.B := LREAL_TO_REAL(Car_pos.Position.c.B);
GVL.BaseRely.C := LREAL_TO_REAL(Car_pos.Position.c.C);
2.2.2.5 TcpDecode
//作者： AlongWu
PROGRAM TcpDecode
VAR CONSTANT
emptyReceiveBuffer :ARRAY [0..999] OF BYTE; //复位ReceiveBuffer
emptyJogGroup :ARRAY [0..5] OF BOOL; //复位Jog开关组
END_VAR
VAR
B_Reply :BOOL; //回复动作标志
locMoveCmd :LocMoveCmd; //位置移动控制命令实例
UI_CmdLen :UINT; //命令字节数量
UI_ReadLen :UINT; //接收字节总数
UI_Rx_Addr :UINT; //解析字节地址
UI_Tx_Addr :UINT; //发送缓存字节地址
UI_Tx_Len :UINT; //发送字节数量
END_VAR
//复位变量
B_Reply :=FALSE;
UI_Rx_Addr :=0;
UI_Tx_Addr :=0;
UI_Tx_Len :=0;
UI_CmdLen :=0;
//获得接收总字节数
UI_ReadLen := ULINT_TO_UINT(GVL.UL_RxSize);
(*
通过while，处理TCP的粘包，和分包。
*)
WHILE UI_Rx_Addr < UI_ReadLen DO
UI_CmdLen := BYTE_TO_UINT(GVL.Tcp_abyRx[UI_Rx_Addr]); //指令格式的 第一个字节是 命令长度。
//命令最后字节位超过接收总字节数，命令不全，退出。
IF (UI_Rx_Addr + UI_CmdLen) > UI_ReadLen THEN
EXIT ;
END_IF
//根据命令的命令码，解析
CASE GVL.Tcp_abyRx[UI_Rx_Addr+1] OF
1:
//TCP循环查询回复报文
GVL.BaseRely.cmd :=1;
GVL.BaseRely.len :=56;
MEM.MemMove(pSource:= ADR(GVL.BaseRely), pDestination:= ADR(GVL.Tcp_abyTx)+UI_Tx_Addr, uiNumberOfBytes:=56);
B_Reply:=TRUE;
UI_Tx_Addr:=UI_Tx_Addr+56;
UI_Tx_Len := UI_Tx_Len+56;
2:
//坐标PTP移动
GVL.GroupJogEnable :=FALSE;
GVL.JogForwardGroup := emptyJogGroup;
GVL.JogBackwardGroup := emptyJogGroup;
MEM.MemMove(pSource:= ADR(GVL.Tcp_abyRx)+UI_Rx_Addr, pDestination:= ADR(locMoveCmd), uiNumberOfBytes:=28);
GVL.GroupMoveAbsTarget_SMC_POS_REF.c.X := locMoveCmd.x;
GVL.GroupMoveAbsTarget_SMC_POS_REF.c.Y := locMoveCmd.y;
GVL.GroupMoveAbsTarget_SMC_POS_REF.c.Z := locMoveCmd.z;
GVL.GroupMoveAbsTarget_SMC_POS_REF.c.A := locMoveCmd.a;
GVL.GroupMoveAbsTarget_SMC_POS_REF.c.B := locMoveCmd.b;
GVL.GroupMoveAbsTarget_SMC_POS_REF.c.C := locMoveCmd.c;
GVL.MotionType := MotionType_Enum.Point;
GVL.B_MotionAct := TRUE;
3:
//坐标直线插补
GVL.GroupJogEnable :=FALSE;
GVL.JogForwardGroup := emptyJogGroup;
GVL.JogBackwardGroup := emptyJogGroup;
MEM.MemMove(pSource:= ADR(GVL.Tcp_abyRx)+UI_Rx_Addr, pDestination:= ADR(locMoveCmd), uiNumberOfBytes:=28);
GVL.GroupMoveAbsTarget_SMC_POS_REF.c.X := locMoveCmd.x;
GVL.GroupMoveAbsTarget_SMC_POS_REF.c.Y := locMoveCmd.y;
GVL.GroupMoveAbsTarget_SMC_POS_REF.c.Z := locMoveCmd.z;
GVL.GroupMoveAbsTarget_SMC_POS_REF.c.A := locMoveCmd.a;
GVL.GroupMoveAbsTarget_SMC_POS_REF.c.B := locMoveCmd.b;
GVL.GroupMoveAbsTarget_SMC_POS_REF.c.C := locMoveCmd.c;
GVL.MotionType := MotionType_Enum.Line;
GVL.B_MotionAct := TRUE;
4:
//轴PTP移动
GVL.GroupJogEnable :=FALSE;
GVL.JogForwardGroup := emptyJogGroup;
GVL.JogBackwardGroup := emptyJogGroup;
MEM.MemMove(pSource:= ADR(GVL.Tcp_abyRx)+UI_Rx_Addr, pDestination:= ADR(locMoveCmd), uiNumberOfBytes:=28);
GVL.GroupMoveAbsTarget_SMC_POS_REF.a.a0 := locMoveCmd.x;
GVL.GroupMoveAbsTarget_SMC_POS_REF.a.a1 := locMoveCmd.y;
GVL.GroupMoveAbsTarget_SMC_POS_REF.a.a2 := locMoveCmd.z;
GVL.GroupMoveAbsTarget_SMC_POS_REF.a.a3 := locMoveCmd.a;
GVL.GroupMoveAbsTarget_SMC_POS_REF.a.a4 := locMoveCmd.b;
GVL.GroupMoveAbsTarget_SMC_POS_REF.a.a5 := locMoveCmd.c;
GVL.MotionType := MotionType_Enum.Axis;
GVL.B_MotionAct := TRUE;
20:
//Jog使能/失能
IF GVL.Tcp_abyRx[UI_Rx_Addr+2] = 0 THEN
GVL.GroupJogEnable :=FALSE;
GVL.JogForwardGroup := emptyJogGroup;
GVL.JogBackwardGroup := emptyJogGroup;
ELSE
GVL.GroupJogEnable :=TRUE;
END_IF
21:
//Jog前进开关组赋值 TRUE
IF GVL.GroupJogEnable THEN
CASE GVL.Tcp_abyRx[UI_Rx_Addr+2] OF
1:
GVL.JogForwardGroup[0]:=TRUE;
2:
GVL.JogForwardGroup[1]:=TRUE;
3:
GVL.JogForwardGroup[2]:=TRUE;
4:
GVL.JogForwardGroup[3]:=TRUE;
5:
GVL.JogForwardGroup[4]:=TRUE;
6:
GVL.JogForwardGroup[5]:=TRUE;
ELSE
//nothing
END_CASE
END_IF
22:
//Jog前进开关组赋值 FALSE
IF GVL.GroupJogEnable THEN
CASE GVL.Tcp_abyRx[UI_Rx_Addr+2] OF
1:
GVL.JogForwardGroup[0]:=FALSE;
2:
GVL.JogForwardGroup[1]:=FALSE;
3:
GVL.JogForwardGroup[2]:=FALSE;
4:
GVL.JogForwardGroup[3]:=FALSE;
5:
GVL.JogForwardGroup[4]:=FALSE;
6:
GVL.JogForwardGroup[5]:=FALSE;
ELSE
//nothing
END_CASE
END_IF
23:
//Jog后退开关组赋值 TRUE
IF GVL.GroupJogEnable THEN
CASE GVL.Tcp_abyRx[UI_Rx_Addr+2] OF
1:
GVL.JogBackwardGroup[0]:=TRUE;
2:
GVL.JogBackwardGroup[1]:=TRUE;
3:
GVL.JogBackwardGroup[2]:=TRUE;
4:
GVL.JogBackwardGroup[3]:=TRUE;
5:
GVL.JogBackwardGroup[4]:=TRUE;
6:
GVL.JogBackwardGroup[5]:=TRUE;
ELSE
//nothing
END_CASE
END_IF
24:
//Jog后退开关组赋值 FALSE
IF GVL.GroupJogEnable THEN
CASE GVL.Tcp_abyRx[UI_Rx_Addr+2] OF
1:
GVL.JogBackwardGroup[0]:=FALSE;
2:
GVL.JogBackwardGroup[1]:=FALSE;
3:
GVL.JogBackwardGroup[2]:=FALSE;
4:
GVL.JogBackwardGroup[3]:=FALSE;
5:
GVL.JogBackwardGroup[4]:=FALSE;
6:
GVL.JogBackwardGroup[5]:=FALSE;
ELSE
//nothing
END_CASE
END_IF
26:
//Jog模式切换
IF GVL.Tcp_abyRx[UI_Rx_Addr+2] = 1 THEN
GVL.GroupJogMode := TRUE;
ELSE
GVL.GroupJogMode := FALSE;
END_IF
80:
//轴组减速停车
GVL.B_GroupHalt:=TRUE;
81:
//轴组继续运动
GVL.B_GroupContinue:=TRUE;
99:
//轴组复位
GVL.B_ResetBt:=TRUE;
//
ELSE
//
END_CASE
//更新解析地址
UI_Rx_Addr:= UI_Rx_Addr + UI_CmdLen;
END_WHILE
//更新发送字节数量和复位接收缓存
GVL.UD_TcpSendSize := UINT_TO_UDINT(UI_Tx_Len);
GVL.Tcp_abyRx := emptyReceiveBuffer;
//判断回复动作
IF B_Reply THEN
GVL.B_TcpSend:=TRUE;
END_IF三、总结
整个项目比较简单，QT和CodeSys都是调用系统的函数。Codesys的Robotics库非常强大和方便。选用QT平台做上位机程序和动作仿真，主要是QtQuick3D在模型变换方面十分便捷，利用自定义Geometroy设置好臂的中心的位置，通过模型旋转即可实现相应臂以及臂组的摆动。