975 lines
24 KiB
C++
975 lines
24 KiB
C++
#include "CaptureCoordinator.h"
|
||
|
||
TwoMotionCaptureCoordinator::TwoMotionCaptureCoordinator(
|
||
IrisMultiMotorController* motorCtrl,
|
||
QObject* parent)
|
||
: QObject(parent)
|
||
, m_motorCtrl(motorCtrl)
|
||
, m_isRunning(false)
|
||
, m_isValidCapturing(false)
|
||
{
|
||
//这些信号槽是按照逻辑顺序的
|
||
connect(this, SIGNAL(moveTo(int, double, double, int)),
|
||
m_motorCtrl, SLOT(moveTo(int, double, double, int)));
|
||
connect(this, SIGNAL(moveTo(const std::vector<double>, const std::vector<double>, int)),
|
||
m_motorCtrl, SLOT(moveTo(const std::vector<double>, const std::vector<double>, int)));
|
||
connect(this, &TwoMotionCaptureCoordinator::stopMotorSignal, m_motorCtrl, &IrisMultiMotorController::stop);
|
||
|
||
connect(m_motorCtrl, &IrisMultiMotorController::motorStopSignal,
|
||
this, &TwoMotionCaptureCoordinator::handlePositionReached);
|
||
//connect(m_motorCtrl, &IrisMultiMotorController::moveFailed,
|
||
// this, &TwoMotionCaptureCoordinator::handleError);
|
||
}
|
||
|
||
TwoMotionCaptureCoordinator::~TwoMotionCaptureCoordinator()
|
||
{
|
||
|
||
}
|
||
|
||
void TwoMotionCaptureCoordinator::start(QVector<PathLine> pathLines)
|
||
{
|
||
//QMutexLocker locker(&m_dataMutex);
|
||
|
||
if (m_isRunning)
|
||
{
|
||
emit errorOccurred("Sequence already running");
|
||
std::cout << "already running" << std::endl;
|
||
return;
|
||
}
|
||
|
||
getLocBeforeStart();
|
||
|
||
m_pathLines = pathLines;
|
||
|
||
m_isMoving2XMin = false;
|
||
m_isMoving2XMax = false;
|
||
m_isMoving2XStartLoc = false;
|
||
|
||
m_isMoving2YTargeLoc = false;
|
||
m_isMoving2YStartLoc = false;
|
||
m_isImagerFrameNumberMeet = false;
|
||
|
||
m_retryTimesMoving2XMin = 0;
|
||
m_retryTimesMoving2XMax = 0;
|
||
m_retryTimesMoving2YTargeLoc = 0;
|
||
|
||
m_isRunning = true;
|
||
m_numCurrentPathLine = 0;
|
||
processNextPathLine();
|
||
}
|
||
|
||
void TwoMotionCaptureCoordinator::stop()
|
||
{
|
||
if (!m_isRunning) return;
|
||
|
||
//QMutexLocker locker(&m_dataMutex);
|
||
|
||
std::cout << "The user manually stops the collection! " << std::endl;
|
||
savePathLinesToCsv();
|
||
|
||
emit finishRecordLineNumSignal(m_numCurrentPathLine);
|
||
emit stopRecordHSISignal(m_numCurrentPathLine);
|
||
|
||
move2LocBeforeStart();
|
||
|
||
emit sequenceComplete(1);
|
||
}
|
||
|
||
void TwoMotionCaptureCoordinator::getLocBeforeStart()
|
||
{
|
||
QEventLoop loop;
|
||
bool received = false;
|
||
|
||
QTimer timer;
|
||
timer.setSingleShot(true);
|
||
connect(&timer, &QTimer::timeout, &loop, &QEventLoop::quit);
|
||
|
||
QMetaObject::Connection conn = QObject::connect(m_motorCtrl, &IrisMultiMotorController::locationSignal,
|
||
[&](std::vector<double> pos) {
|
||
m_locBeforeStart = pos;
|
||
std::cout << "start pos: "<< pos[0] <<", " << pos[1] << std::endl;
|
||
received = true;
|
||
loop.quit();
|
||
});
|
||
|
||
QMetaObject::invokeMethod(m_motorCtrl, "getLoc", Qt::QueuedConnection);
|
||
timer.start(3000);
|
||
|
||
loop.exec();
|
||
|
||
std::vector<double> pos;
|
||
pos.push_back(0);
|
||
pos.push_back(0);
|
||
m_locBeforeStart = pos;
|
||
|
||
disconnect(conn);
|
||
}
|
||
|
||
void TwoMotionCaptureCoordinator::getRecordState()
|
||
{
|
||
emit recordState(m_isRunning);
|
||
}
|
||
|
||
void TwoMotionCaptureCoordinator::move2LocBeforeStart()
|
||
{
|
||
std::cout << "\nmove2LocBeforeStart." << std::endl;
|
||
|
||
PathLine& tmp = m_pathLines[0];
|
||
std::vector<double> speed;
|
||
speed.push_back(tmp.speedTargetXMinPosition);
|
||
speed.push_back(tmp.speedTargetYPosition);
|
||
emit moveTo(m_locBeforeStart, speed, 1000);
|
||
|
||
m_isValidCapturing = false;
|
||
|
||
m_isMoving2XMin = false;
|
||
m_isMoving2XMax = false;
|
||
m_isMoving2YTargeLoc = false;
|
||
|
||
m_isMoving2XStartLoc = true;
|
||
m_isMoving2YStartLoc = true;
|
||
}
|
||
|
||
QVector<PathLine> TwoMotionCaptureCoordinator::pathLines() const
|
||
{
|
||
//QMutexLocker locker(&m_dataMutex);
|
||
return m_pathLines;
|
||
}
|
||
|
||
double TwoMotionCaptureCoordinator::getTimeDiffMinutes(QDateTime startTime, QDateTime endTime)
|
||
{
|
||
qint64 diffMillis = startTime.msecsTo(endTime);
|
||
double diffMinutes = (double)diffMillis / 60000;//min
|
||
|
||
return diffMinutes;
|
||
}
|
||
|
||
bool TwoMotionCaptureCoordinator::savePathLinesToCsv(QString filename)
|
||
{
|
||
//QMutexLocker locker(&m_dataMutex);
|
||
if (filename.isEmpty())
|
||
{
|
||
FileOperation* fileOperation = new FileOperation();
|
||
string directory = fileOperation->getDirectoryOfExe();
|
||
QDateTime now = QDateTime::currentDateTime();
|
||
QString format1 = "yyyyMMdd_HHmmss";
|
||
QString fileNameTmp = now.toString("yyyyMMdd_HHmmss");
|
||
|
||
filename = QDir::cleanPath(QString::fromStdString(directory) + QDir::separator() + "pathLines" + QDir::separator() + fileNameTmp + "_pathLines.csv");
|
||
}
|
||
|
||
QDir dir = QFileInfo(filename).absoluteDir();
|
||
|
||
// 如果目录不存在，则递归创建
|
||
if (!dir.exists()) {
|
||
if (!dir.mkpath(".")) {
|
||
qWarning() << "Failed to create directory:" << dir.path();
|
||
return false;
|
||
}
|
||
}
|
||
|
||
QFile file(filename);
|
||
if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
QTextStream out(&file);
|
||
out << "timestamp1,timestamp2,timestamp3,time consuming(min),targetYPosition,actualYPosition,targetXMinPosition,actualXMinPosition,targetXMaxPosition,actualXMaxPosition\n";
|
||
|
||
for (const auto& data : m_pathLines)
|
||
{
|
||
out << data.timestamp1.toString("yyyy-MM-dd HH:mm:ss.zzz") << ","
|
||
<< data.timestamp2.toString("yyyy-MM-dd HH:mm:ss.zzz") << ","
|
||
<< data.timestamp3.toString("yyyy-MM-dd HH:mm:ss.zzz") << ","
|
||
<< QString::number(getTimeDiffMinutes(data.timestamp2, data.timestamp3), 'f', 4) << ","
|
||
<< QString::number(data.targetYPosition, 'f', 4) << ","
|
||
<< QString::number(data.actualYPosition, 'f', 4) << ","
|
||
<< QString::number(data.targetXMinPosition, 'f', 4) << ","
|
||
<< QString::number(data.actualXMinPosition, 'f', 4) << ","
|
||
<< QString::number(data.targetXMaxPosition, 'f', 4) << ","
|
||
<< QString::number(data.actualXMaxPosition, 'f', 4)
|
||
<< "\n";
|
||
}
|
||
|
||
file.close();
|
||
return true;
|
||
}
|
||
|
||
void TwoMotionCaptureCoordinator::handlePositionReached(int motorID, double pos)
|
||
{
|
||
if (!m_isRunning) return;
|
||
|
||
//QMutexLocker locker(&m_dataMutex);
|
||
|
||
if (motorID == 1)//y马达
|
||
{
|
||
if (m_isMoving2YTargeLoc)
|
||
{
|
||
PathLine& tmp = m_pathLines[m_numCurrentPathLine];
|
||
|
||
double threshold = getThre(tmp.targetYPosition, pos);
|
||
|
||
if (threshold > 5)
|
||
{
|
||
//没到准确位置，再次给马达发送命令
|
||
if (m_retryTimesMoving2YTargeLoc < m_retryLimit)
|
||
{
|
||
m_retryTimesMoving2YTargeLoc++;
|
||
|
||
std::cout << "Y motor Moving2YTargeLoc error. Retry..." << std::endl;
|
||
emit moveTo(1, tmp.targetYPosition, tmp.speedTargetYPosition, 1000);
|
||
return;
|
||
}
|
||
}
|
||
m_retryTimesMoving2YTargeLoc = 0;
|
||
|
||
tmp.actualYPosition = pos;
|
||
|
||
m_isMoving2YTargeLoc = false;
|
||
|
||
std::cout << "y motor is reached!!!! " << std::endl;
|
||
startRecordHsi();
|
||
|
||
return;
|
||
}
|
||
|
||
if (m_isMoving2YStartLoc)
|
||
{
|
||
m_isMoving2YStartLoc = false;
|
||
isBack2Origin();
|
||
|
||
return;
|
||
}
|
||
}
|
||
if (motorID == 0)//x马达
|
||
{
|
||
if (m_isMoving2XMin)
|
||
{
|
||
PathLine& tmp = m_pathLines[m_numCurrentPathLine];
|
||
|
||
double threshold = getThre(tmp.targetXMinPosition, pos);
|
||
|
||
if (threshold > 5)
|
||
{
|
||
//没到准确位置，再次给马达发送命令
|
||
if (m_retryTimesMoving2XMin < m_retryLimit)
|
||
{
|
||
m_retryTimesMoving2XMin++;
|
||
|
||
std::cout << "X motor Moving2XMin error. Retry..." << std::endl;
|
||
emit moveTo(0, tmp.targetXMinPosition, tmp.speedTargetXMinPosition, 1000);
|
||
return;
|
||
}
|
||
}
|
||
m_retryTimesMoving2XMin = 0;
|
||
|
||
tmp.actualXMinPosition = pos;
|
||
|
||
m_isMoving2XMin = false;
|
||
|
||
std::cout << "x motor is reached!!!! " << std::endl;
|
||
startRecordHsi();
|
||
|
||
return;
|
||
}
|
||
|
||
if (m_isMoving2XMax)
|
||
{
|
||
PathLine& tmp = m_pathLines[m_numCurrentPathLine];
|
||
|
||
double threshold = getThre(tmp.targetXMaxPosition, pos);
|
||
|
||
if (threshold > 5 && !m_isImagerFrameNumberMeet)//马达没到准确位置 && 【非】光谱仪因帧数限制主动停止采集
|
||
{
|
||
//没到准确位置，再次给马达发送命令
|
||
if (m_retryTimesMoving2XMax < m_retryLimit)
|
||
{
|
||
m_retryTimesMoving2XMax++;
|
||
|
||
std::cout << "X motor Moving2XMax error. Retry..." << std::endl;
|
||
emit moveTo(0, tmp.targetXMaxPosition, tmp.speedTargetXMaxPosition, 1000);
|
||
return;
|
||
}
|
||
}
|
||
m_retryTimesMoving2XMax = 0;
|
||
|
||
tmp.actualXMaxPosition = pos;
|
||
tmp.timestamp3 = QDateTime::currentDateTime();
|
||
|
||
std::cout << "Line " << m_numCurrentPathLine << " time span(min):" << getTimeDiffMinutes(tmp.timestamp2, tmp.timestamp3) << std::endl;
|
||
|
||
//停止采集高光谱数据
|
||
emit finishRecordLineNumSignal(m_numCurrentPathLine);
|
||
emit stopRecordHSISignal(m_numCurrentPathLine);
|
||
|
||
m_isMoving2XMax = false;
|
||
m_isImagerFrameNumberMeet = false;
|
||
m_numCurrentPathLine++;
|
||
processNextPathLine();
|
||
|
||
return;
|
||
}
|
||
|
||
if (m_isMoving2XStartLoc)
|
||
{
|
||
m_isMoving2XStartLoc = false;
|
||
isBack2Origin();
|
||
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
|
||
double TwoMotionCaptureCoordinator::getThre(double targetLoc,double actualLoc)
|
||
{
|
||
double targetLocTmp;
|
||
if (targetLoc == 0)
|
||
{
|
||
targetLocTmp = 0.001;
|
||
}
|
||
else
|
||
{
|
||
targetLocTmp = targetLoc;
|
||
}
|
||
double thre = abs(targetLoc - actualLoc) / targetLocTmp * 100;
|
||
|
||
return thre;
|
||
}
|
||
|
||
void TwoMotionCaptureCoordinator::startRecordHsi()
|
||
{
|
||
if (!m_isRunning) return;
|
||
|
||
//QMutexLocker locker(&m_dataMutex);
|
||
|
||
if (!m_isMoving2XMin && !m_isMoving2YTargeLoc)
|
||
{
|
||
//开始采集高光谱数据
|
||
PathLine &tmp = m_pathLines[m_numCurrentPathLine];
|
||
tmp.timestamp2 = QDateTime::currentDateTime();
|
||
std::cout << "start recording hsi, moving to " << tmp.targetXMaxPosition << std::endl;
|
||
|
||
m_isMoving2XMax = true;
|
||
emit moveTo(0, tmp.targetXMaxPosition, tmp.speedTargetXMaxPosition, 1000);
|
||
|
||
emit startRecordHSISignal(m_numCurrentPathLine);
|
||
emit startRecordLineNumSignal(m_numCurrentPathLine);
|
||
}
|
||
}
|
||
|
||
void TwoMotionCaptureCoordinator::isBack2Origin()
|
||
{
|
||
if (!m_isRunning) return;
|
||
|
||
//QMutexLocker locker(&m_dataMutex);
|
||
|
||
if (!m_isMoving2XStartLoc && !m_isMoving2YStartLoc)
|
||
{
|
||
m_isRunning = false;
|
||
|
||
emit back2OriginSignal();
|
||
}
|
||
|
||
}
|
||
|
||
void TwoMotionCaptureCoordinator::handleCaptureCompleteWhenFrameNumberMeet()
|
||
{
|
||
m_isImagerFrameNumberMeet = true;
|
||
emit stopMotorSignal(0);
|
||
}
|
||
|
||
void TwoMotionCaptureCoordinator::handleError(const QString& error)
|
||
{
|
||
//QMutexLocker locker(&m_dataMutex);
|
||
m_isRunning = false;
|
||
emit errorOccurred(error);
|
||
}
|
||
|
||
void TwoMotionCaptureCoordinator::processNextPathLine()
|
||
{
|
||
if (!m_isRunning) return;
|
||
|
||
int numPathLines = m_pathLines.size();
|
||
|
||
if (numPathLines == 0)
|
||
{
|
||
move2LocBeforeStart();
|
||
return;
|
||
}
|
||
|
||
if (m_isMoving2YTargeLoc || m_isMoving2XMin)
|
||
{
|
||
return;
|
||
}
|
||
|
||
if (m_numCurrentPathLine > numPathLines - 1)
|
||
{
|
||
std::cout << "\nAll path lines is finished! " << std::endl;
|
||
|
||
move2LocBeforeStart();
|
||
savePathLinesToCsv();
|
||
|
||
emit sequenceComplete(0);
|
||
|
||
return;
|
||
}
|
||
|
||
std::cout << "\nNew path line: " << m_numCurrentPathLine << std::endl;
|
||
emit gotoRecordLineNumSignal(m_numCurrentPathLine);
|
||
m_isValidCapturing = true;
|
||
|
||
PathLine &tmp = m_pathLines[m_numCurrentPathLine];
|
||
tmp.timestamp1 = QDateTime::currentDateTime();
|
||
|
||
std::vector<double> loc;
|
||
loc.push_back(tmp.targetXMinPosition);
|
||
loc.push_back(tmp.targetYPosition);
|
||
std::vector<double> speed;
|
||
speed.push_back(tmp.speedTargetXMinPosition);
|
||
speed.push_back(tmp.speedTargetYPosition);
|
||
|
||
m_isMoving2YTargeLoc = true;
|
||
m_isMoving2XMin = true;
|
||
emit moveTo(loc, speed, 1000);
|
||
}
|
||
|
||
OneMotionCaptureCoordinator::OneMotionCaptureCoordinator(
|
||
IrisMultiMotorController* motorCtrl,
|
||
ImagerOperationBase* cameraCtrl,
|
||
QObject* parent)
|
||
: QObject(parent)
|
||
, m_motorCtrl(motorCtrl)
|
||
, m_cameraCtrl(cameraCtrl)
|
||
, m_isRunning(false)
|
||
{
|
||
connect(this, SIGNAL(moveTo(int, double, double, int)),
|
||
m_motorCtrl, SLOT(moveTo(int, double, double, int)));
|
||
connect(this, SIGNAL(moveSignal(int, bool, double, int)), m_motorCtrl, SLOT(move(int, bool, double, int)));
|
||
connect(this, &OneMotionCaptureCoordinator::stopMotorSignal, m_motorCtrl, &IrisMultiMotorController::stop);
|
||
|
||
connect(m_motorCtrl, &IrisMultiMotorController::motorStopSignal,
|
||
this, &OneMotionCaptureCoordinator::handleMotorStoped);
|
||
//connect(m_motorCtrl, &IrisMultiMotorController::moveFailed,
|
||
// this, &OneMotionCaptureCoordinator::handleError);
|
||
|
||
connect(this, &OneMotionCaptureCoordinator::startRecordHSISignal,
|
||
m_cameraCtrl, &ImagerOperationBase::start_record);
|
||
connect(this, &OneMotionCaptureCoordinator::stopRecordHSISignal,
|
||
m_cameraCtrl, &ImagerOperationBase::stop_record);
|
||
connect(m_cameraCtrl, &ImagerOperationBase::RecordFinishedSignal_WhenFrameNumberMeet,
|
||
this, &OneMotionCaptureCoordinator::handleCaptureCompleteWhenFrameNumberMeet);
|
||
}
|
||
|
||
OneMotionCaptureCoordinator::~OneMotionCaptureCoordinator()
|
||
{
|
||
disconnect(m_motorCtrl, &IrisMultiMotorController::motorStopSignal,
|
||
this, &OneMotionCaptureCoordinator::handleMotorStoped);
|
||
}
|
||
|
||
void OneMotionCaptureCoordinator::startStepMotion(OneMotionCapturePathLine pathLine)//这个函数为啥被调用了2次？
|
||
{
|
||
QMutexLocker locker(&m_dataMutex);
|
||
|
||
if (m_isRunning)
|
||
{
|
||
emit errorOccurred("Sequence already running");
|
||
return;
|
||
}
|
||
|
||
m_isRunning = true;
|
||
m_pathLine = pathLine;
|
||
|
||
getLocBeforeStart();
|
||
m_pathLine.startPosition = m_locBeforeStart[0];
|
||
m_pathLine.timestamp1 = QDateTime::currentDateTime();
|
||
|
||
//移动马达并开始采集高光谱
|
||
emit moveSignal(0, false, m_pathLine.speedRecord, 1000);
|
||
emit startRecordHSISignal();
|
||
}
|
||
|
||
void OneMotionCaptureCoordinator::stopStepMotion()
|
||
{
|
||
QMutexLocker locker(&m_dataMutex);
|
||
|
||
if (m_cameraCtrl != nullptr)
|
||
{
|
||
m_cameraCtrl->stop_record();
|
||
}
|
||
|
||
emit stopMotorSignal(0);
|
||
m_isHypercamStopRecord = true;
|
||
}
|
||
|
||
void OneMotionCaptureCoordinator::handleCaptureCompleteWhenFrameNumberMeet()
|
||
{
|
||
emit stopMotorSignal(0);
|
||
m_isHypercamStopRecord = true;
|
||
}
|
||
|
||
void OneMotionCaptureCoordinator::getLocBeforeStart()
|
||
{
|
||
QEventLoop loop;
|
||
bool received = false;
|
||
|
||
QTimer timer;
|
||
timer.setSingleShot(true);
|
||
connect(&timer, &QTimer::timeout, &loop, &QEventLoop::quit);
|
||
|
||
QMetaObject::Connection conn = QObject::connect(m_motorCtrl, &IrisMultiMotorController::locationSignal,
|
||
[&](std::vector<double> pos) {
|
||
m_locBeforeStart = pos;
|
||
received = true;
|
||
loop.quit();
|
||
});
|
||
|
||
QMetaObject::invokeMethod(m_motorCtrl, "getLoc", Qt::QueuedConnection);
|
||
timer.start(3000);
|
||
|
||
loop.exec();
|
||
|
||
disconnect(conn);
|
||
|
||
std::vector<double> pos;
|
||
pos.push_back(0);
|
||
m_locBeforeStart = pos;
|
||
}
|
||
|
||
void OneMotionCaptureCoordinator::move2LocBeforeStart()
|
||
{
|
||
std::cout << "\nmove2LocBeforeStart." << std::endl;
|
||
|
||
emit moveTo(0, m_locBeforeStart[0], m_pathLine.speedBack, 1000);
|
||
|
||
m_isRunning = false;
|
||
}
|
||
|
||
bool OneMotionCaptureCoordinator::saveToCsv(const QString& filename)
|
||
{
|
||
//QMutexLocker locker(&m_dataMutex);
|
||
|
||
QFile file(filename);
|
||
if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
QTextStream out(&file);
|
||
out << "startTime,stopTime,startPosition,stopPosition\n";
|
||
|
||
out << m_pathLine.timestamp1.toString("yyyy-MM-dd HH:mm:ss.zzz") << ","
|
||
<< m_pathLine.timestamp2.toString("yyyy-MM-dd HH:mm:ss.zzz") << ","
|
||
<< QString::number(m_pathLine.startPosition, 'f', 4) << ","
|
||
<< QString::number(m_pathLine.stopPosition, 'f', 4) << "\n";
|
||
|
||
file.close();
|
||
return true;
|
||
}
|
||
|
||
void OneMotionCaptureCoordinator::handleMotorStoped(int motorID, double pos)
|
||
{
|
||
QMutexLocker locker(&m_dataMutex);
|
||
|
||
if (m_isHypercamStopRecord == true)
|
||
{
|
||
m_isHypercamStopRecord = false;
|
||
|
||
// 记录位置信息
|
||
m_pathLine.stopPosition = pos;
|
||
m_pathLine.timestamp2 = QDateTime::currentDateTime();
|
||
|
||
//光谱仪停止采集，马达回到初始位置
|
||
emit stopRecordHSISignal();
|
||
if (m_cameraCtrl != nullptr)
|
||
{
|
||
m_cameraCtrl->stop_record();
|
||
}
|
||
move2LocBeforeStart();
|
||
|
||
emit sequenceComplete_cam_stop_before_motorback(0);
|
||
}
|
||
else
|
||
{
|
||
// emit sequenceComplete last: the slot connected to it may delete this object,
|
||
// so no member access is allowed after this point.
|
||
emit sequenceComplete(0);
|
||
}
|
||
}
|
||
|
||
void OneMotionCaptureCoordinator::handleCaptureComplete(double index)
|
||
{
|
||
if (!m_isRunning) return;
|
||
|
||
QMutexLocker locker(&m_dataMutex);
|
||
|
||
}
|
||
|
||
void OneMotionCaptureCoordinator::handleError(const QString& error)
|
||
{
|
||
QMutexLocker locker(&m_dataMutex);
|
||
m_isRunning = false;
|
||
emit errorOccurred(error);
|
||
}
|
||
|
||
DarkAndWhiteCaptureCoordinator::DarkAndWhiteCaptureCoordinator(
|
||
int model,
|
||
IrisMultiMotorController* motorCtrl,
|
||
ImagerOperationBase* cameraCtrl,
|
||
QObject* parent)
|
||
: QObject(parent)
|
||
, m_model(model)
|
||
, m_motorCtrl(motorCtrl)
|
||
, m_cameraCtrl(cameraCtrl)
|
||
, m_isRunning(false)
|
||
{
|
||
connect(this, SIGNAL(moveTo(int, double, double, int)),
|
||
m_motorCtrl, SLOT(moveTo(int, double, double, int)));
|
||
connect(this, SIGNAL(moveSignal(int, bool, double, int)), m_motorCtrl, SLOT(move(int, bool, double, int)));
|
||
connect(this, &DarkAndWhiteCaptureCoordinator::stopMotorSignal, m_motorCtrl, &IrisMultiMotorController::stop);
|
||
|
||
connect(m_motorCtrl, &IrisMultiMotorController::motorStopSignal,
|
||
this, &DarkAndWhiteCaptureCoordinator::handleMotorStoped);
|
||
|
||
if (m_model == 0)//dark
|
||
{
|
||
connect(this, &DarkAndWhiteCaptureCoordinator::startRecordHSISignal,
|
||
m_cameraCtrl, &ImagerOperationBase::record_dark);
|
||
connect(m_cameraCtrl, &ImagerOperationBase::RecordDarlFinishSignal,
|
||
this, &DarkAndWhiteCaptureCoordinator::handleCaptureCompleteWhenFrameNumberMeet);
|
||
}
|
||
else if(m_model == 1)//white
|
||
{
|
||
connect(this, &DarkAndWhiteCaptureCoordinator::startRecordHSISignal,
|
||
m_cameraCtrl, &ImagerOperationBase::record_white);
|
||
connect(m_cameraCtrl, &ImagerOperationBase::RecordWhiteFinishSignal,
|
||
this, &DarkAndWhiteCaptureCoordinator::handleCaptureCompleteWhenFrameNumberMeet);
|
||
}
|
||
|
||
}
|
||
|
||
DarkAndWhiteCaptureCoordinator::~DarkAndWhiteCaptureCoordinator()
|
||
{
|
||
|
||
}
|
||
|
||
void DarkAndWhiteCaptureCoordinator::startStepMotion(double speed)
|
||
{
|
||
QMutexLocker locker(&m_dataMutex);
|
||
|
||
if (m_isRunning)
|
||
{
|
||
return;
|
||
}
|
||
|
||
m_isRunning = true;
|
||
|
||
m_speed = speed;
|
||
|
||
getLocBeforeStart();
|
||
|
||
//移动马达并开始采集高光谱
|
||
emit moveSignal(0, false, m_speed, 1000);
|
||
emit startRecordHSISignal();
|
||
}
|
||
|
||
void DarkAndWhiteCaptureCoordinator::handleCaptureCompleteWhenFrameNumberMeet()
|
||
{
|
||
emit stopMotorSignal(0);
|
||
}
|
||
|
||
void DarkAndWhiteCaptureCoordinator::getLocBeforeStart()
|
||
{
|
||
QEventLoop loop;
|
||
bool received = false;
|
||
|
||
QTimer timer;
|
||
timer.setSingleShot(true);
|
||
connect(&timer, &QTimer::timeout, &loop, &QEventLoop::quit);
|
||
|
||
QMetaObject::Connection conn = QObject::connect(m_motorCtrl, &IrisMultiMotorController::locationSignal,
|
||
[&](std::vector<double> pos) {
|
||
m_locBeforeStart = pos;
|
||
received = true;
|
||
loop.quit();
|
||
});
|
||
|
||
QMetaObject::invokeMethod(m_motorCtrl, "getLoc", Qt::QueuedConnection);
|
||
timer.start(3000);
|
||
|
||
loop.exec();
|
||
|
||
disconnect(conn);
|
||
}
|
||
|
||
void DarkAndWhiteCaptureCoordinator::move2LocBeforeStart()
|
||
{
|
||
std::cout << "\nmove2LocBeforeStart." << std::endl;
|
||
|
||
emit moveTo(0, m_locBeforeStart[0], m_speed, 1000);
|
||
|
||
m_isRunning = false;
|
||
}
|
||
|
||
void DarkAndWhiteCaptureCoordinator::handleMotorStoped(int motorID, double pos)
|
||
{
|
||
QMutexLocker locker(&m_dataMutex);
|
||
|
||
if (!m_isRunning) return;
|
||
|
||
move2LocBeforeStart();
|
||
}
|
||
|
||
void DarkAndWhiteCaptureCoordinator::handleCaptureComplete(double index)
|
||
{
|
||
QMutexLocker locker(&m_dataMutex);
|
||
}
|
||
|
||
|
||
TwoMotor1PosCoordinator::TwoMotor1PosCoordinator(
|
||
IrisMultiMotorController* motorCtrl,
|
||
QObject* parent)
|
||
: QObject(parent)
|
||
, m_motorCtrl(motorCtrl)
|
||
, m_isMoving2Target(false)
|
||
, m_isMoving2Origin(false)
|
||
, m_targetX(0)
|
||
, m_targetY(0)
|
||
, m_speedX(0)
|
||
, m_speedY(0)
|
||
, m_actualX(0)
|
||
, m_actualY(0)
|
||
, m_retryTimesX(0)
|
||
, m_retryTimesY(0)
|
||
, m_xReached(false)
|
||
, m_yReached(false)
|
||
{
|
||
//因为IrisMultiMotorController::moveTo有多个重载版本，所以使用信号槽连接时需要使用SIGNAL和SLOT宏来指定参数类型，避免编译器无法推断出正确的函数签名。
|
||
//connect(this, &TwoMotor1PosCoordinator::moveTo, m_motorCtrl, &IrisMultiMotorController::moveTo);//这行代码会报错，因为moveTo有多个重载版本，编译器无法推断出正确的函数签名。
|
||
connect(this, SIGNAL(moveTo(int, double, double, int)), m_motorCtrl, SLOT(moveTo(int, double, double, int)));
|
||
connect(this, SIGNAL(moveTo(const std::vector<double>, const std::vector<double>, int)), m_motorCtrl, SLOT(moveTo(const std::vector<double>, const std::vector<double>, int)));
|
||
|
||
|
||
connect(this, &TwoMotor1PosCoordinator::stopMotorSignal, m_motorCtrl, &IrisMultiMotorController::stop);
|
||
|
||
connect(m_motorCtrl, &IrisMultiMotorController::motorStopSignal, this, &TwoMotor1PosCoordinator::handlePositionReached);
|
||
}
|
||
|
||
TwoMotor1PosCoordinator::~TwoMotor1PosCoordinator()
|
||
{
|
||
|
||
}
|
||
|
||
void TwoMotor1PosCoordinator::moveToTarget(double xTarget, double yTarget, double xSpeed, double ySpeed)
|
||
{
|
||
m_isMoving2Target = true;
|
||
m_isMoving2Origin = false;
|
||
|
||
moveToTargetPrivate(xTarget, yTarget, xSpeed, ySpeed);
|
||
}
|
||
|
||
void TwoMotor1PosCoordinator::back2origin()
|
||
{
|
||
m_isMoving2Target = false;
|
||
m_isMoving2Origin = true;
|
||
|
||
moveToTargetPrivate(0, 0, m_speedX, m_speedY);
|
||
}
|
||
|
||
void TwoMotor1PosCoordinator::moveToTargetPrivate(double xTarget, double yTarget, double xSpeed, double ySpeed)
|
||
{
|
||
m_targetX = xTarget;
|
||
m_targetY = yTarget;
|
||
m_speedX = xSpeed;
|
||
m_speedY = ySpeed;
|
||
|
||
m_xReached = false;
|
||
m_yReached = false;
|
||
m_retryTimesX = 0;
|
||
m_retryTimesY = 0;
|
||
|
||
std::vector<double> loc = { m_targetX, m_targetY };
|
||
std::vector<double> speed = { m_speedX, m_speedY };
|
||
|
||
std::cout << "TwoMotor1PosCoordinator: moving to (" << m_targetX << ", " << m_targetY << ")" << std::endl;
|
||
emit moveTo(loc, speed, 1000);
|
||
}
|
||
|
||
void TwoMotor1PosCoordinator::handlePositionReached(int motorID, double pos)
|
||
{
|
||
if (!m_isMoving2Target && !m_isMoving2Origin)
|
||
{
|
||
return;
|
||
}
|
||
|
||
double errorRateThreshold = 5;
|
||
if (motorID == 0)
|
||
{
|
||
m_actualX = pos;
|
||
|
||
double errorRate = getErrorRate(m_targetX, m_actualX);
|
||
|
||
if (errorRate > errorRateThreshold && m_retryTimesX < m_retryLimit)
|
||
{
|
||
m_retryTimesX++;
|
||
std::cout << "X motor retry " << m_retryTimesX << ", target: " << m_targetX << ", actual: " << m_actualX << std::endl;
|
||
emit moveTo(0, m_targetX, m_speedX, 1000);
|
||
return;
|
||
}
|
||
m_retryTimesX = 0;
|
||
m_xReached = true;
|
||
|
||
std::cout << "X motor reached: " << m_actualX << std::endl;
|
||
}
|
||
else if (motorID == 1)
|
||
{
|
||
m_actualY = pos;
|
||
|
||
double errorRate = getErrorRate(m_targetY, m_actualY);
|
||
if (errorRate > errorRateThreshold && m_retryTimesY < m_retryLimit)
|
||
{
|
||
m_retryTimesY++;
|
||
std::cout << "Y motor retry " << m_retryTimesY << ", target: " << m_targetY << ", actual: " << m_actualY << std::endl;
|
||
emit moveTo(1, m_targetY, m_speedY, 1000);
|
||
return;
|
||
}
|
||
m_retryTimesY = 0;
|
||
m_yReached = true;
|
||
|
||
std::cout << "Y motor reached: " << m_actualY << std::endl;
|
||
}
|
||
|
||
if (checkArrival())
|
||
{
|
||
std::cout << "TwoMotor1PosCoordinator: Arrived at (" << m_actualX << ", " << m_actualY << ")" << std::endl;
|
||
|
||
if (m_isMoving2Origin)
|
||
{
|
||
m_isMoving2Origin = false;
|
||
|
||
emit back2OriginSignal();
|
||
}
|
||
|
||
if (m_isMoving2Target)
|
||
{
|
||
m_isMoving2Target = false;
|
||
|
||
emit ArrivalSignal(m_actualX, m_actualY);
|
||
}
|
||
}
|
||
}
|
||
|
||
double TwoMotor1PosCoordinator::getErrorRate(double targetLoc, double actualLoc)
|
||
{
|
||
double targetLocTmp;
|
||
if (targetLoc == 0)
|
||
{
|
||
targetLocTmp = 0.001;
|
||
}
|
||
else
|
||
{
|
||
targetLocTmp = targetLoc;
|
||
}
|
||
double errorRate = abs(targetLoc - actualLoc) / targetLocTmp * 100;
|
||
|
||
return errorRate;
|
||
}
|
||
|
||
bool TwoMotor1PosCoordinator::checkArrival()
|
||
{
|
||
return m_xReached && m_yReached;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
//---------------------------------------------------------------------------------------------------------------------------------------------
|
||
|
||
OneMotionCoordinator::OneMotionCoordinator(IrisMultiMotorController* motorCtrl, QObject* parent)
|
||
: QObject(parent)
|
||
, m_motorCtrl(motorCtrl)
|
||
, m_targetPosition(0)
|
||
, m_speed(0)
|
||
, m_actualPosition(0)
|
||
, m_isMoving(false)
|
||
, m_retryTimes(0)
|
||
, m_reached(false)
|
||
{
|
||
connect(this, SIGNAL(moveTo(int, double, double, int)), m_motorCtrl, SLOT(moveTo(int, double, double, int)));
|
||
connect(m_motorCtrl, &IrisMultiMotorController::motorStopSignal, this, &OneMotionCoordinator::handlePositionReached);
|
||
}
|
||
|
||
OneMotionCoordinator::~OneMotionCoordinator()
|
||
{
|
||
}
|
||
|
||
void OneMotionCoordinator::moveToTarget(double position, double speed)
|
||
{
|
||
QMutexLocker locker(&m_dataMutex);
|
||
m_targetPosition = position;
|
||
m_speed = speed;
|
||
m_retryTimes = 0;
|
||
m_reached = false;
|
||
m_isMoving = true;
|
||
|
||
qDebug() << "OneMotionCoordinator: moving to" << position;
|
||
emit moveTo(0, position, speed, 1000);
|
||
}
|
||
|
||
void OneMotionCoordinator::handlePositionReached(int motorID, double position)
|
||
{
|
||
if (!m_isMoving || motorID != 0)
|
||
{
|
||
return;
|
||
}
|
||
|
||
QMutexLocker locker(&m_dataMutex);
|
||
m_actualPosition = position;
|
||
|
||
double errorRate = getErrorRate(m_targetPosition, m_actualPosition);
|
||
|
||
if (errorRate > 5 && m_retryTimes < m_retryLimit)
|
||
{
|
||
m_retryTimes++;
|
||
qDebug() << "OneMotionCoordinator: retry" << m_retryTimes << ", target:" << m_targetPosition << ", actual:" << m_actualPosition;
|
||
emit moveTo(0, m_targetPosition, m_speed, 1000);
|
||
return;
|
||
}
|
||
|
||
m_retryTimes = 0;
|
||
m_reached = true;
|
||
m_isMoving = false;
|
||
|
||
qDebug() << "OneMotionCoordinator: Arrived at" << m_actualPosition;
|
||
|
||
emit sequenceComplete(0);
|
||
emit ArrivalSignal(m_actualPosition);
|
||
}
|
||
|
||
double OneMotionCoordinator::getErrorRate(double targetLoc, double actualLoc)
|
||
{
|
||
double targetLocTmp;
|
||
if (targetLoc == 0)
|
||
{
|
||
targetLocTmp = 0.001;
|
||
}
|
||
else
|
||
{
|
||
targetLocTmp = targetLoc;
|
||
}
|
||
double errorRate = abs(targetLoc - actualLoc) / targetLocTmp * 100;
|
||
|
||
return errorRate;
|
||
}
|