初级代码游戏的专栏介绍与文章目录-CSDN博客
我的github：codetoys，所有代码都将会位于ctfc库中。已经放入库中我会指出在库中的位置。
这些代码大部分以Linux为目标但部分代码是纯C++的，可以在任何平台上使用。
源码指引：github源码指引_初级代码游戏的博客-CSDN博客
C#是我多年以来的业余爱好，新搞的东西能用C#的就用C#了。
新松机械臂报告状态的端口是2001，每100毫秒报告一次状态，每次报告数据长度1468字节。
这个端口只用来报告状态，控制则使用2000端口。
获取状态非常简单，连接上去，读取数据，按照格式拆分数据即可。具体数据数据格式见开发手册。
目录
一、C++头文件
二、main函数
三、核心代码
四、运行
一、C++头文件
只需要使用C++头文件DucoRobat.h即可。其实并不需要这个头文件，本例程中引入是因为借用了里面定义好的状态结构而已：
// 机器人相关信息
struct RobotStatusList
{
std::vector<double> jointExpectPosition; // 目标关节位置
std::vector<double> jointExpectVelocity; // 目标角速度
std::vector<double> jointExpectAccelera; // 目标角加速度
std::vector<double> jointActualPosition; // 实际关节位置
std::vector<double> jointActualVelocity; // 实际角速度
std::vector<double> jointActualAccelera; // 实际角加速度
std::vector<double> jointActualCurrent; // 实际关节电流
std::vector<double> jointTemperature; // 时间关节温度
std::vector<double> driverTemperature; // 未使用
std::vector<double> cartExpectPosition; // 目标末端位姿
std::vector<double> cartExpectVelocity; // 目标末端速度
std::vector<double> cartExpectAccelera; // 目标末端加速度
std::vector<double> cartActualPosition; // 实际末端位姿
std::vector<double> cartActualVelocity; // 实际末端速度
std::vector<double> cartActualAccelera; // 实际末端加速度
std::vector<bool> slaveReady; // 从站状态
bool collision; // 是否发生碰撞
int8_t collisionAxis; // 发生碰撞的关节
bool emcStopSignal; // 未使用
int8_t robotState; // 机器人状态
int32_t robotError; // 机器人错误码
};
二、main函数
int main(int argc, char** argv)
{
if (!InitActiveApp("DOUCO", 1024 * 1024 * 10, argc, argv))return 1;
thelog << "新松机器人 TCP及IP接口" << endi;
WORD sockVersion = MAKEWORD(1, 1);
WSADATA wsaData;
if (WSAStartup(sockVersion, &wsaData) != 0)
{
return 0;
}
bool bExitCmd = false;
thread t(CMyDUCO::VirtualServer, &bExitCmd);//模拟服务
SleepSeconds(1);
CMyDUCO myduco;
string ip = "127.0.0.1";//正式使用修改为设备实际IP
thelog << "连接 " << ip << " ......"<< endi;
if (!myduco.Recv(ip.c_str()))
{
thelog << "连接到 " << ip << " 失败" << ende;
}
else
{
myduco.ProcessFrame();
myduco.Print();
myduco.Save();
}
while (true)SleepSeconds(5);
bExitCmd = true;
t.join();
WSACleanup();
return 0;
}
这个代码默认访问自带的模拟服务，实际使用修改目标IP（代码中已标注）。
三、核心代码
// DUCO.h : 新松机器人 TCP及IP接口
//
#include <iostream>
//#include "DucoCobot.h"
//using namespace DucoRPC;
using namespace std;
#include "env/myUtil.h"
#include "function/htmldoc.h"
#include "function/mysocket.h"
using namespace ns_my_std;
#pragma comment(lib,"ws2_32.lib")
// 机器人相关信息
struct RobotStatusList
{
std::vector<double> jointExpectPosition; // 目标关节位置
std::vector<double> jointExpectVelocity; // 目标角速度
std::vector<double> jointExpectAccelera; // 目标角加速度
std::vector<double> jointActualPosition; // 实际关节位置
std::vector<double> jointActualVelocity; // 实际角速度
std::vector<double> jointActualAccelera; // 实际角加速度
std::vector<double> jointActualCurrent; // 实际关节电流
std::vector<double> jointTemperature; // 时间关节温度
std::vector<double> driverTemperature; // 未使用
std::vector<double> cartExpectPosition; // 目标末端位姿
std::vector<double> cartExpectVelocity; // 目标末端速度
std::vector<double> cartExpectAccelera; // 目标末端加速度
std::vector<double> cartActualPosition; // 实际末端位姿
std::vector<double> cartActualVelocity; // 实际末端速度
std::vector<double> cartActualAccelera; // 实际末端加速度
std::vector<bool> slaveReady; // 从站状态
bool collision; // 是否发生碰撞
int8_t collisionAxis; // 发生碰撞的关节
bool emcStopSignal; // 未使用
int8_t robotState; // 机器人状态
int32_t robotError; // 机器人错误码
};
constexpr int frame_size = 1468;
class CMyDUCO
{
private:
int32_t _getUInt(int byte_pos)
{
if (4 != sizeof(int32_t))throw "4 != sizeof(int32_t)";
int32_t tmp;
memcpy(&tmp, frame + byte_pos, sizeof(int32_t));
return tmp;
}
float _getFloat(int byte_pos)
{
if (4 != sizeof(float))throw "4 != sizeof(float)";
float tmp;
memcpy(&tmp, frame + byte_pos, sizeof(float));
return tmp;
}
void _FloatX(vector<double>& vd7, int count, int byte_pos)
{
vd7.clear();
for (int i = 0; i < count; ++i)
{
vd7.push_back(_getFloat(byte_pos + i * 4));
}
}
union
{
char frame[frame_size];
float _;
}m_frame;
char* frame = m_frame.frame;
RobotStatusList m_RobotStatusList;
public:
bool Save()
{
CEasyFile file;
for (int i = 0; i < 100; ++i)
{
char buf[256];
sprintf(buf, "data%03d.dat", i);
if (file.IsFileExist(buf))continue;
return file.WriteFile(buf, frame, frame_size);
}
thelog << "已经存在的文件太多" << ende;
return false;
}
void ProcessFrame()
{
// 目标关节位置
_FloatX(m_RobotStatusList.jointExpectPosition, 7, 112);
// 目标角速度
_FloatX(m_RobotStatusList.jointExpectVelocity, 7, 140);
// 目标角加速度
_FloatX(m_RobotStatusList.jointExpectAccelera, 7, 168);
// 实际关节位置
_FloatX(m_RobotStatusList.jointActualPosition, 7, 0);
// 实际角速度
_FloatX(m_RobotStatusList.jointActualVelocity, 7, 28);
// 实际角加速度
_FloatX(m_RobotStatusList.jointActualAccelera, 7, 56);
// 实际关节电流
_FloatX(m_RobotStatusList.jointActualCurrent, 7, 252);
// 实际关节温度
_FloatX(m_RobotStatusList.jointTemperature, 7, 224);
// 目标末端位姿
_FloatX(m_RobotStatusList.cartExpectPosition, 6, 464);
// 目标末端速度
_FloatX(m_RobotStatusList.cartExpectVelocity, 6, 488);
// 目标末端加速度
_FloatX(m_RobotStatusList.cartExpectAccelera, 6, 512);
// 实际末端位姿
_FloatX(m_RobotStatusList.cartActualPosition, 6, 368);
// 实际末端速度
_FloatX(m_RobotStatusList.cartActualVelocity, 6, 392);
// 实际末端加速度
_FloatX(m_RobotStatusList.cartActualAccelera, 6, 416);
// 是否发生碰撞
m_RobotStatusList.collision = (1 == frame[1452]);
// 发生碰撞的关节
m_RobotStatusList.collisionAxis = frame[1453];
// 机器人状态
m_RobotStatusList.robotState = frame[1449];
// 机器人错误码
m_RobotStatusList.robotError = _getUInt(1456);
}
void Random()
{
static int base = 0;
float* p = &m_frame._;
for (int i = 0; i < frame_size / 4; ++i)
{
p[i] = base+i;
}
++base;
// 是否发生碰撞
frame[1452] = 1;
// 发生碰撞的关节
frame[1453] = 6;
// 机器人状态
frame[1449] = 9;
// 机器人错误码
*(int32_t*)&frame[1456] = 10;
}
void _PrintV(CHtmlDoc::CHtmlTable2 &table,vector<double> & vectorD, char const* title)
{
table.AddLine();
table.AddData(title);
for (auto v : vectorD)
{
table.AddData(v, 2);
}
}
void Print()
{
CHtmlDoc::CHtmlTable2 table;
table.AddCol("名称");
for (int i = 0; i < 7; ++i)
{
char buf[32];
sprintf(buf, "关节%d", i + 1);
table.AddCol(buf, CHtmlDoc::CHtmlDoc_DATACLASS_RIGHT);
}
_PrintV(table, m_RobotStatusList.jointExpectPosition, "目标关节位置");
_PrintV(table, m_RobotStatusList.jointExpectVelocity, "目标角速度");
_PrintV(table, m_RobotStatusList.jointExpectAccelera, "目标角加速度");
_PrintV(table, m_RobotStatusList.jointActualPosition, "实际关节位置");
_PrintV(table, m_RobotStatusList.jointActualVelocity, "实际角速度");
_PrintV(table, m_RobotStatusList.jointActualAccelera, "实际角加速度");
_PrintV(table, m_RobotStatusList.jointActualCurrent, "实际关节电流");
_PrintV(table, m_RobotStatusList.jointTemperature, "时间关节温度");
_PrintV(table, m_RobotStatusList.driverTemperature, "未使用");
_PrintV(table, m_RobotStatusList.cartExpectPosition, "目标末端位姿");
_PrintV(table, m_RobotStatusList.cartExpectVelocity, "目标末端速度");
_PrintV(table, m_RobotStatusList.cartExpectAccelera, "目标末端加速度");
_PrintV(table, m_RobotStatusList.cartActualPosition, "实际末端位姿");
_PrintV(table, m_RobotStatusList.cartActualVelocity, "实际末端速度");
_PrintV(table, m_RobotStatusList.cartActualAccelera, "实际末端加速度");
theLog << table.MakeTextTable() << endi;
theLog << endl << m_RobotStatusList.collision << " 是否发生碰撞" << endl;
theLog << (int)m_RobotStatusList.collisionAxis << " 发生碰撞的关节" << endl;
theLog << (int)m_RobotStatusList.robotState << " 机器人状态" << endl;
theLog << m_RobotStatusList.robotError << " 机器人错误码" << endi;
}
//接收数据
bool Recv(char const * ip)
{
CMySocket s;
if (!s.Connect(ip, 2001))
{
thelog << "连接失败 "<<ip << ende;
return false;
}
thelog << "连接成功 " << ip << endi;
int count = 0;
while (count != frame_size)
{
long readCount = 0;
if (!s.Recv(frame + count, frame_size - count, &readCount))
{
thelog << "接收失败 " << ip << ende;
s.Close();
return false;
}
thelog << readCount<<" 收到数据 " << count << endi;
count += readCount;
}
thelog << "收到数据 " << count << endi;
s.Close();
return count == frame_size;
}
//虚拟服务
static void VirtualServer(bool * pExitCmd)
{
thelog.SetSource("TEST SERVER");
CMySocket s;
if (!s.Listen(2001))
{
thelog << "监听端口2001失败" << ende;
exit(1);
}
thelog << "服务已创建" << endi;
while (!*pExitCmd)
{
bool tmp_bool;
if (!s.IsSocketReadReady(1, tmp_bool))
{
thelog << "IsSocketReadReady失败" << ende;
exit(1);
}
if (!tmp_bool)continue;
CMySocket client_socket = s.Accept();
if (!client_socket.IsConnected())
{
thelog << "Accept失败" << ende;
exit(1);
}
thelog << "Accept成功" << endi;
CMyDUCO myDUCO;
myDUCO.Random();
client_socket.Send(myDUCO.frame, frame_size);
client_socket.Close();
}
s.Close();
thelog << "服务结束" << endi;
}
};
这个代码已经把所需的结构直接放进来了，不需要包含DucoRobat.h。
代码主要功能：
- Save 保存到当前目录
- ProcessFrame 解析收到的数据
- Random 用于模拟服务生成数据，其实并不是随机，而是递增
- Print 输出表格
- Recv 接收数据
- VirtualServer 内置模拟服务
四、运行
运行起来结果如下：
[05-30 11:13:55][应用][信息][C:\working\IoT\DUCO\main_t.cpp: 12(main)][ 0.00]新松机器人 TCP及IP接口
[05-30 11:13:55][TEST SERVER][12824- 1][信息][C:\working\IoT\DUCO\myDUCO.h: 230(VirtualServer)][ 0.02]服务已创建
[05-30 11:13:56][应用][12824][信息][C:\working\IoT\DUCO\main_t.cpp: 27(main)][ 1.01]连接 127.0.0.1 ......
[05-30 11:13:56][应用][12824][信息][C:\working\IoT\DUCO\myDUCO.h: 199(Recv)][ 1.01]连接成功 127.0.0.1
[05-30 11:13:56][TEST SERVER][12824- 1][信息][C:\working\IoT\DUCO\myDUCO.h: 247(VirtualServer)][ 1.01]Accept成功
[05-30 11:13:56][应用][12824][信息][C:\working\IoT\DUCO\myDUCO.h: 211(Recv)][ 1.01]1468 收到数据 0
[05-30 11:13:56][应用][12824][信息][C:\working\IoT\DUCO\myDUCO.h: 215(Recv)][ 1.01]收到数据 1468
[05-30 11:13:56][应用][12824][信息]
名称 关节1 关节2 关节3 关节4 关节5 关节6 关节7
-------------- ------ ------ ------ ------ ------ ------ -----
目标关节位置 28.00 29.00 30.00 31.00 32.00 33.00 34.00
目标角速度 35.00 36.00 37.00 38.00 39.00 40.00 41.00
目标角加速度 42.00 43.00 44.00 45.00 46.00 47.00 48.00
实际关节位置 0.00 1.00 2.00 3.00 4.00 5.00 6.00
实际角速度 7.00 8.00 9.00 10.00 11.00 12.00 13.00
实际角加速度 14.00 15.00 16.00 17.00 18.00 19.00 20.00
实际关节电流 63.00 64.00 65.00 66.00 67.00 68.00 69.00
时间关节温度 56.00 57.00 58.00 59.00 60.00 61.00 62.00
未使用
目标末端位姿 116.00 117.00 118.00 119.00 120.00 121.00
目标末端速度 122.00 123.00 124.00 125.00 126.00 127.00
目标末端加速度 128.00 129.00 130.00 131.00 132.00 133.00
实际末端位姿 92.00 93.00 94.00 95.00 96.00 97.00
实际末端速度 98.00 99.00 100.00 101.00 102.00 103.00
实际末端加速度 104.00 105.00 106.00 107.00 108.00 109.00
-------------- ------ ------ ------ ------ ------ ------ -----
[05-30 11:13:56][应用][12824][信息]
1 是否发生碰撞
6 发生碰撞的关节
9 机器人状态
10 机器人错误码
其实相当简单。
（这里是文档结束）
