221 lines
8.7 KiB
C++
221 lines
8.7 KiB
C++
#include <triggerManager.h>
|
|
#include <backEndManager.h>
|
|
#include <threadPoolManager.h>
|
|
#include <backStruct.h>
|
|
#include <cameraManager.h>
|
|
#include <jsonParsing.h>
|
|
#define debugTesting 1
|
|
static correctionStruct* correctionData;
|
|
void triggerManager::decodeCameraCMD(QByteArray byte)
|
|
{
|
|
#if debugTesting
|
|
//if (checkTrigger(1, 2, byte) == true)
|
|
{
|
|
std::shared_ptr <detData> data = dManager->insertDetData("Camera1", "Thread3", "TEST", 0);
|
|
data->correctionData = correctionData;
|
|
_threadManager->addTask(data);//添加任务
|
|
auto camera = _cameraManager->cameras.find("123");
|
|
if(camera != _cameraManager->cameras.end())
|
|
camera->second->Process(data);
|
|
}
|
|
//if (checkTrigger(2, 2, byte) == true)
|
|
{
|
|
std::shared_ptr <detData> data = dManager->insertDetData("Camera1", "Thread2", "TEST", 1);
|
|
data->correctionData = correctionData;
|
|
_threadManager->addTask(data);//添加任务
|
|
//auto camera = _cameraManager->cameras.find("456");
|
|
//if (camera != _cameraManager->cameras.end())
|
|
// camera->second->Process(data);
|
|
}
|
|
return;
|
|
#endif
|
|
#if debugTesting == 0
|
|
if (byte.isEmpty())
|
|
return;
|
|
double cameraData = 0;
|
|
if (checkTrigger(1, 1, byte) == true)
|
|
{
|
|
correctionData = new correctionStruct();
|
|
correctionData->in_X_X0Y = { -258.71,-447.8,-447.2 };
|
|
correctionData->in_Y_X0Y = { -1099.67,-1133.42,-938.83 };
|
|
correctionData->in_Z_X0Y = { 711.77,711.62,711.730000 };
|
|
correctionData->in_Z_X0Y_Value = { 63.9795,64.5333,65.859300 };
|
|
|
|
correctionData->in_X_X0Z = { -402.3,-455.91,-236.75 };
|
|
correctionData->in_Y_X0Z = { -539.46,-536.21,-536.2 };
|
|
correctionData->in_Z_X0Z = { -418.88,425.12,425.13 };
|
|
correctionData->in_Y_X0Z_Value = { 118.15,-228.19,-256.5 };
|
|
|
|
correctionData->in_X_Y0Z = { 395.250000,395.23,328.34 };
|
|
correctionData->in_Y_Y0Z = { -1147.29,-1147.28,-940.82 };
|
|
correctionData->in_Z_Y0Z = { 118.15,-228.19,-256.5 };
|
|
correctionData->in_X_Y0Z_Value = { -36.652200,-36.7302,41.0436 };
|
|
std::shared_ptr <detData> data = dManager->insertDetData("Camera1", "correctionDataThread", "TEST", 0);
|
|
data->correctionData = correctionData;
|
|
_threadManager->addTask(data);//添加任务
|
|
correctionData = nullptr;
|
|
}
|
|
if (checkTrigger(1, 2, byte) == true)
|
|
{
|
|
std::shared_ptr <detData> data = dManager->insertDetData("Camera1", "Thread3", "TEST", 0);
|
|
data->correctionData = correctionData;
|
|
_threadManager->addTask(data);//添加任务
|
|
auto camera = _cameraManager->cameras.find("123");
|
|
camera->second -> Process(data);
|
|
}
|
|
if (checkTrigger(2, 2, byte) == true)
|
|
{
|
|
std::shared_ptr <detData> data = dManager->insertDetData("Camera1", "Thread3", "TEST", 0);
|
|
data->correctionData = correctionData;
|
|
_threadManager->addTask(data);//添加任务
|
|
auto camera = _cameraManager->cameras.find("456");
|
|
camera->second->Process(data);
|
|
}
|
|
//总流程开始，机器人发送1至寄存器259
|
|
|
|
//上位机确认OK之后，清零寄存器259，并发送1至寄存器260
|
|
//如产品型号为Product_1且为正面，则上位机发送1至寄存器256，发送0至寄存器257
|
|
//之后机器人确认之后需发送测量点位总数具体值至寄存器258
|
|
if (checkTrigger(259, 1, byte) == true)
|
|
{
|
|
sendFinsMsg("write_short_259_0");//触发信号清零
|
|
sendFinsMsg("write_short_260_1");//开始检测信号
|
|
}
|
|
|
|
//机器人激光当前点位 264 1...9
|
|
//机器人到达激光点位1时，寄存器261至1，264至1，并将机器人在激光点位1坐标XYZ按照定义规则，保存进寄存器265~270
|
|
|
|
//上位机确认机器人到达点位1(264)时，确认OK后，寄存器262至1，261至0，取点完成后，发送寄存器263至1
|
|
if (checkTrigger(261, 1, byte) == true)//读取请求地址
|
|
{
|
|
int armPos = getValue(264, byte);
|
|
|
|
int xValue1 = getValue(265, byte);
|
|
int xValue2 = getValue(266, byte);
|
|
int xValue3 = getValue(267, byte);
|
|
|
|
int yValue1 = getValue(268, byte);
|
|
int yValue2 = getValue(269, byte);
|
|
int yValue3 = getValue(270, byte);
|
|
|
|
int zValue1 = getValue(271, byte);
|
|
int zValue2 = getValue(272, byte);
|
|
int zValue3 = getValue(273, byte);
|
|
|
|
double xValue = (xValue1 == 1 ? 1 : -1) * ((double)xValue2 + ((double)xValue3) / (std::pow(10, to_string(xValue3).length())));
|
|
double yValue = (yValue1 == 1 ? 1 : -1) * ((double)yValue2 + ((double)yValue3) / (std::pow(10, to_string(yValue3).length())));
|
|
double zValue = (zValue1 == 1 ? 1 : -1) * ((double)zValue2 + ((double)zValue3) / (std::pow(10, to_string(zValue3).length())));
|
|
double rangeData = _cameraManager->cameras.find("相机的名字")->second->getCameraValue();
|
|
|
|
if (armPos == 1)//第一次
|
|
correctionData = new correctionStruct();
|
|
if (correctionData != nullptr)
|
|
{
|
|
if (armPos <= 3)
|
|
{
|
|
correctionData->in_X_X0Y.push_back(xValue);
|
|
correctionData->in_Y_X0Y.push_back(yValue);
|
|
correctionData->in_Z_X0Y.push_back(zValue);
|
|
correctionData->in_Z_X0Y_Value.push_back(rangeData);
|
|
}
|
|
else if (armPos <= 6)
|
|
{
|
|
correctionData->in_X_X0Z.push_back(xValue);
|
|
correctionData->in_Y_X0Z.push_back(yValue);
|
|
correctionData->in_Z_X0Z.push_back(zValue);
|
|
correctionData->in_Y_X0Z_Value.push_back(rangeData);
|
|
}
|
|
else if (armPos <= 9)
|
|
{
|
|
correctionData->in_X_Y0Z.push_back(xValue);
|
|
correctionData->in_Y_Y0Z.push_back(yValue);
|
|
correctionData->in_Z_Y0Z.push_back(zValue);
|
|
correctionData->in_X_Y0Z_Value.push_back(rangeData);
|
|
}
|
|
}
|
|
sendFinsMsg("write_short_261_0");//清零
|
|
sendFinsMsg("write_short_262_1");
|
|
sendFinsMsg("write_short_263_1");
|
|
|
|
//上位机校正完成，将寄存器274置为1，并将结果按照定义XYZ规则放入310~336中
|
|
if (armPos == 9)
|
|
{
|
|
std::shared_ptr <detData> data = dManager->insertDetData("Camera1", "correctionDataThread", "TEST", 0);
|
|
data->correctionData = correctionData;
|
|
_threadManager->addTask(data);//添加任务
|
|
correctionData = nullptr;
|
|
}
|
|
}
|
|
|
|
//机器人到达激光点位9时，寄存器261至1，264至9，并将机器人在激光点位9坐标XYZ按照定义规则，保存进寄存器265~270
|
|
|
|
if (checkTrigger(300, 1, byte) == true)
|
|
{
|
|
sendFinsMsg("write_short_300_0");//触发信号清零
|
|
sendFinsMsg("write_short_301_1");//开始检测信号
|
|
int Pos = getValue(303, byte);
|
|
}
|
|
|
|
|
|
#endif
|
|
}
|
|
|
|
void triggerManager::init(backEndManager* input)
|
|
{
|
|
_backEndManager = input;
|
|
dManager = _backEndManager->_detDataManager;
|
|
_cameraManager = _backEndManager->_cameraManager;
|
|
_threadManager = _backEndManager->_threadManager;
|
|
_jsonConfiguration = _backEndManager->_jsonConfiguration;
|
|
|
|
triggerFileName = _jsonConfiguration->tcp.moduleName;
|
|
IpAndPort = _jsonConfiguration->tcp.finstcp_init;
|
|
//triggerInitStr = QByteArray("sleeptime_%1_readnum_%2_writetype_ushort");
|
|
QLibrary* m_lib = new QLibrary(triggerFileName);
|
|
bool flag = m_lib->load();
|
|
if (!m_lib->isLoaded())
|
|
{
|
|
//emit _backEndManager->sendErrorToMainwindow(dllNotFound);
|
|
emit _backEndManager->sendInitString("找不到"+triggerFileName);
|
|
return;
|
|
}
|
|
|
|
if (flag)
|
|
{
|
|
CreateFunction createFun = (CreateFunction)m_lib->resolve("create");
|
|
ReleaseFunction releaseFun = (ReleaseFunction)m_lib->resolve("destory");
|
|
bool initFailure = false;
|
|
if (createFun && releaseFun)
|
|
{
|
|
trigger = createFun();
|
|
destory_finstcp = releaseFun;
|
|
QByteArray initByte = QByteArray::fromStdString("sleeptime_20_readnum_300_writetype_ushort");
|
|
trigger->reciveData(vec_mat, QByteArray(IpAndPort.toUtf8()));
|
|
emit _backEndManager->sendInitString(triggerFileName + "连接到" + IpAndPort);
|
|
initFailure=trigger->initParas(initByte);
|
|
}
|
|
|
|
if(initFailure==false)
|
|
{
|
|
emit _backEndManager->sendErrorToMainwindow(triggerInitFailure);
|
|
delete m_lib;
|
|
m_lib = nullptr;
|
|
isInit = false;
|
|
emit _backEndManager->sendInitString(triggerFileName + "初始化失败");
|
|
return;
|
|
}
|
|
else
|
|
emit _backEndManager->sendInitString(triggerFileName + "成功");
|
|
}
|
|
this->start();
|
|
}
|
|
|
|
int triggerManager::getValue(int dmPos, QByteArray byte)
|
|
{
|
|
int triggerPos = (dmPos - _backEndManager->_jsonConfiguration->tcp.startPos) * 2;
|
|
if (byte.size() / 2 < triggerPos)
|
|
return false;
|
|
int val1 = byte.at(triggerPos) * 256;
|
|
int val2 = static_cast<unsigned char>(byte.at(triggerPos + 1));
|
|
return val1 + val2;
|
|
} |