#include "stdafx.h" #include "TomoMotionModel.h" #include "IMachineryManager.h" #include "MotionStages.h" #include "TomoMotionStageArgs.h" #include "ITubeAngleController.h" #include "ITubeHeightController.h" #include "ISensorADController.h" #include "ISensorEncoderController.h" #include "IExposureController.h" #include "FeedbackDefine.h" #include "generatorMove.hpp" #include "IPositionManager.h" #include "DigitalTwinLogger.h" #include "ConfigureExposureDelayTime.h" #include "TubeLineMotionSwitchController.h" #include "ConfigurerMotion.h" #include "ConfigurerWS.h" #include "ConfigurerTomoMotionLimitation.h" using namespace DIOS::Dev::Detail::MachineryECOM; TomoMotionModel::TomoMotionModel() :m_StageName(""), m_stageArgs(new TomoMotionStageArgs()), m_coordinates(nullptr), m_machineryManager(nullptr), m_motorTubeAngle(nullptr), m_motorTubeHeight(nullptr), m_adDetectorHeight(nullptr), m_encoderTubeAngle(nullptr), m_encoderTubeHeight(nullptr), m_motorTubeHorizontal(nullptr), m_adTubeHorizontal(nullptr), m_encoderTubeHorizontal(nullptr), m_tubeLineMotionSwitch(nullptr), m_exposure(nullptr), m_warmAngle(0.0f), m_anglePerSlice(0.0f), m_angleSliceCount(0), m_heightSliceCount(0), m_heightStartPosition(0.0f), m_angleStartPosition(0.0f), m_fps(4.0f) { m_heightSlices = new float[TOMO_HEIGHT_SLICE_MAX]{0}; m_anglePeriods = new int[TOMO_HEIGHT_SLICE_MAX]{0}; m_angleSteps = new int[TOMO_HEIGHT_SLICE_MAX]{0}; } TomoMotionModel::~TomoMotionModel() { if (m_stageArgs) { delete m_stageArgs; m_stageArgs = nullptr; } if (m_heightSlices) { delete[] m_heightSlices; m_heightSlices = nullptr; } if (m_anglePeriods) { delete[] m_anglePeriods; m_anglePeriods = nullptr; } if (m_angleSteps) { delete[] m_angleSteps; m_angleSteps = nullptr; } } void TomoMotionModel::ChangeStage(const std::string &stageName) { m_StageName = stageName; OnMotionStage(m_StageName); } std::string TomoMotionModel::GetStageName() { return m_StageName; } IMotionStageArgs *TomoMotionModel::GetStageArgs() { return m_stageArgs; } void TomoMotionModel::Initialize(IMachineryManager *machineryManager, IPositionManager *coordinates) { m_machineryManager = machineryManager; m_coordinates = coordinates; m_motorTubeAngle = (ITubeAngleController *)(m_machineryManager->Resove(CONTROLLER_TUBE_ANGLE)); m_motorTubeHeight = (ITubeHeightController *)(m_machineryManager->Resove(CONTROLLER_TUBE_HEIGHT)); m_adDetectorHeight = (ISensorADController *)(m_machineryManager->Resove(CONTROLLER_DETECTOR_HEIGHT_AD)); m_encoderTubeAngle = (ISensorEncoderController *)(m_machineryManager->Resove(CONTROLLER_TUBE_ANGLE_ENCODER)); m_encoderTubeHeight = (ISensorEncoderController *)(m_machineryManager->Resove(CONTROLLER_TUBE_HEIGHT_ENCODER)); m_exposure = (IExposureController *)(m_machineryManager->Resove(CONTROLLER_EXPOSURE)); m_motorTubeHorizontal = (ITubeHeightController*)(machineryManager->Resove(CONTROLLER_TUBE_HORIZONTAL)); m_encoderTubeHorizontal = (ISensorEncoderController*)(machineryManager->Resove(CONTROLLER_TUBE_HORIZONTAL_ENCODER)); m_tubeLineMotionSwitch = (IOutputController*)(machineryManager->Resove(CONTROLLER_TUBE_LINE_MOTION_SWTICH)); assert(m_motorTubeAngle); assert(m_motorTubeHeight); assert(m_adDetectorHeight); assert(m_encoderTubeAngle); assert(m_encoderTubeHeight); assert(m_exposure); assert(m_motorTubeHorizontal); assert(m_encoderTubeHorizontal); assert(m_tubeLineMotionSwitch); } void TomoMotionModel::LoadMachineryParams(ResDataObject ¶ms) { m_MachineryParams = params; } void TomoMotionModel::LoadModelParams(ResDataObject ¶ms) { m_ModelParams = params; } void TomoMotionModel::SetTechnicalParams(ResDataObject ¶ms) { m_TechnicalParams = params; DigitalTwinLogger::Instance()->ReceiveExamMode("TOMO"); } void TomoMotionModel::OnFeedbackMotionParams(ResDataObject ¶ms) { std::string key((const char *)params["key"]); if (key == TOMO_MODEL_FEEDBACK_KEY_LINEENCODER) { m_FeedbackTubeHeightEncoders.push_back((int)params[key.c_str()]); DigitalTwinLogger::Instance()->ReceiveTomoExposureHeightPosition((int)params[key.c_str()]); } else if (key == TOMO_MODEL_FEEDBACK_KEY_ANGLEENCODER) { m_FeedbackTubeAngleEncoders.push_back((int)params[key.c_str()]); DigitalTwinLogger::Instance()->ReceiveTomoExposureAnglePosition((int)params[key.c_str()]); } } BOOL TomoMotionModel::GetMotionParams(ResDataObject ¶ms) { std::string key = (const char *)params["key"]; if (key == "AngleResult") { params.clear(); int count = 0; for (auto item : m_FeedbackTubeAngleEncoders) { float angle = m_coordinates->ConvertSensorValue(CONTROLLER_TUBE_ANGLE_ENCODER, item); char buffer[10]; memset(buffer, 0, 10); sprintf_s(buffer, "%d", count); params.add(buffer, angle); ++count; } } else if (key == "HeightResult") { params.clear(); int count = 0; for (auto item : m_FeedbackTubeHeightEncoders) { float height = m_coordinates->ConvertSensorValue(CONTROLLER_TUBE_HEIGHT_ENCODER, item); char buffer[10]; memset(buffer, 0, 10); sprintf_s(buffer, "%d", count); params.add(buffer, height * 100); ++count; } } return TRUE; } void TomoMotionModel::OnMotionStage(const std::string &stageName) { if (gmotionLog) gmotionLog->Info("[TomoMotionModel][OnMotionStage]->[Enter][{$}]", stageName.c_str()); if (stageName == TOMO_STAGE_CALCULATE_PARAMS) { MotionStageCalculateParams(); } else if (stageName == TOMO_STAGE_SET_TOMO_SLICE) { MotionStageSetTomoMotionSlice(); } else if (stageName == TOMO_STAGE_MOVETO_END_POS) { MotionStageMovetoEndPos(); } else if (stageName == TOMO_STAGE_CLEAR_PARAMS) { MotionStageClearParams(); } else if (stageName == TOMO_STAGE_MOVETO_START_POS_PRE_CENTER_ADJUST) { MotionStageCenterAdjust(); } else if (stageName == TOMO_STAGE_MOVETO_START_POS) { MotionStageMovetoStartPos(); } else if (stageName == TOMO_STAGE_MOTION_ERROR_STOP) { MotionStageErrorStop(); } else if (stageName == TOMO_STAGE_MOTION_ERROR_RECOVER) { MotionStageErrorRecover(); } else if (stageName == TOMO_STAGE_MOTION_CHANGE_PFS) { m_fps = m_stageArgs->FPS; } else if (stageName == TOMO_STAGE_ADJUST_SID_MOVE) { OnMotionStageMove2SID(m_CurWS); } if (gmotionLog) gmotionLog->Info("[TomoMotionModel][OnMotionStage]->[Exit][{$}]", stageName.c_str()); } void TomoMotionModel::DoMotionStageSetTomoMotionSlice(ISensorEncoderController* pEncoder, string encoderControllerName) { int effectivePeriod = 50; //1.设置球管旋转方向 m_motorTubeAngle->SetRotateOrientation(GetTomoRotateDirection()); //2.设置高度编码器触发同步使能 bool useHeightTriggerSync = (bool)atoi((const char*)m_MachineryParams["UseHeightTriggerSyncBox"]); bool useHeightTriggerAngleRotation = (bool)atoi((const char*)m_MachineryParams["UseHeightTriggerAngleRotation"]); if (useHeightTriggerSync) { auto exposureTriggerID = m_exposure->GetInterfaceID(ID_COMMUNICATE_INTERFACE); pEncoder->ActiveExposureTrigger(exposureTriggerID); } if (useHeightTriggerAngleRotation) { auto rotateTriggerID = m_motorTubeAngle->GetInterfaceID(ID_COMMUNICATE_INTERFACE); pEncoder->ActiveRotateTrigger(rotateTriggerID); } //3.依次设置曝光高度点位关联同步信号 if (useHeightTriggerSync) { int count = m_heightSliceCount - 2; if (m_ModelParams.GetFirstOf("TomoNeedExtraTrigger") >= 0 && (int)atoi(m_ModelParams["TomoNeedExtraTrigger"]) != 0) { count = m_heightSliceCount - 1; } float speed = strtof((const char*)m_TechnicalParams["TomoMotionSpeed"], nullptr); float systemdelayTime = ConfigureExposureDelayTime::GetExposureDelayTimeInSeconds((const char*)m_TechnicalParams["PositionNumber"]); int direct = 1; if (m_heightSlices[0] < m_heightSlices[1]) { direct = -1; } float detaHeight = speed * systemdelayTime * direct; for (int i = 2; i < count; ++i) { if (gbusinessLog) gbusinessLog->Info("[TomoMotionModel][SetExposureTrigger]->[{$:f3} {$:f3} {$:f3}]", m_heightSlices[i] + detaHeight, m_heightSlices[i], detaHeight); DWORD encoder = m_coordinates->ConvertPhysicsValue(encoderControllerName, m_heightSlices[i] + detaHeight); pEncoder->SetExposureTrigger(encoder); } } //4.设置起始曝光点位为触发旋转的点 if (useHeightTriggerAngleRotation) { DWORD encoder = m_coordinates->ConvertPhysicsValue(encoderControllerName, m_heightSlices[2]); pEncoder->SetRotateTrigger(encoder); } //5.依次设置曝光高度点位关联的旋转周期和步长 for (int i = 0; i < m_angleSliceCount; ++i) { int period = m_anglePeriods[i]; int step = m_angleSteps[i]; if (period <= effectivePeriod || period > 0xFFFF) { TPRINTA_ERROR("Error At Tomo Calculation.idx:%d,Period:%u,Effective:%u", period, effectivePeriod); continue; } m_motorTubeAngle->AppendPWM(step, period); } //6.设置曝光触发角度编码器值回传 auto sensortubeHeightID = pEncoder->GetInterfaceID(ID_COMMUNICATE_INTERFACE); m_exposure->ActiveExposureTubeHeightPositionAutoNotify(sensortubeHeightID); auto sensortubeAngleID = m_encoderTubeAngle->GetInterfaceID(ID_COMMUNICATE_INTERFACE); m_exposure->ActiveExposureTubeAnglePositionAutoNotify(sensortubeAngleID); } void TomoMotionModel::MotionStageSetTomoMotionSlice() { if (m_CurWS == WS_WALL) { DoMotionStageSetTomoMotionSlice(m_encoderTubeHeight, CONTROLLER_TUBE_HEIGHT_ENCODER); } else if (m_CurWS == WS_TABLE) { DoMotionStageSetTomoMotionSlice(m_encoderTubeHorizontal, CONTROLLER_TUBE_HORIZONTAL_ENCODER); } //int effectivePeriod = 50; ////1.设置球管旋转方向 //m_motorTubeAngle->SetRotateOrientation(GetTomoRotateDirection()); ////2.设置高度编码器触发同步使能 //bool useHeightTriggerSync = (bool)atoi((const char *)m_MachineryParams["UseHeightTriggerSyncBox"]); //bool useHeightTriggerAngleRotation = (bool)atoi((const char *)m_MachineryParams["UseHeightTriggerAngleRotation"]); //if (useHeightTriggerSync) //{ // auto exposureTriggerID = m_exposure->GetInterfaceID(ID_COMMUNICATE_INTERFACE); // m_encoderTubeHeight->ActiveExposureTrigger(exposureTriggerID); //} //if (useHeightTriggerAngleRotation) //{ // auto rotateTriggerID = m_motorTubeAngle->GetInterfaceID(ID_COMMUNICATE_INTERFACE); // m_encoderTubeHeight->ActiveRotateTrigger(rotateTriggerID); //} ////3.依次设置曝光高度点位关联同步信号 //if (useHeightTriggerSync) //{ // int count = m_heightSliceCount - 2; // if (m_ModelParams.GetFirstOf("TomoNeedExtraTrigger") >= 0 && (int)atoi(m_ModelParams["TomoNeedExtraTrigger"]) != 0) // { // count = m_heightSliceCount - 1; // } // float speed = strtof((const char *)m_TechnicalParams["TomoMotionSpeed"], nullptr); // float systemdelayTime = ConfigureExposureDelayTime::GetExposureDelayTimeInSeconds((const char *)m_TechnicalParams["PositionNumber"]); // int direct = 1; // if (m_heightSlices[0] < m_heightSlices[1]) // { // direct = -1; // } // float detaHeight = speed * systemdelayTime * direct; // for (int i = 2; i < count; ++i) // { // if(gbusinessLog) gbusinessLog->Info("[TomoMotionModel][SetExposureTrigger]->[{$:f3} {$:f3} {$:f3}]", m_heightSlices[i] + detaHeight, m_heightSlices[i], detaHeight); // // DWORD encoder = m_coordinates->ConvertPhysicsValue(CONTROLLER_TUBE_HEIGHT_ENCODER, m_heightSlices[i] + detaHeight); // m_encoderTubeHeight->SetExposureTrigger(encoder); // } //} ////4.设置起始曝光点位为触发旋转的点 //if (useHeightTriggerAngleRotation) //{ // DWORD encoder = m_coordinates->ConvertPhysicsValue(CONTROLLER_TUBE_HEIGHT_ENCODER, m_heightSlices[2]); // m_encoderTubeHeight->SetRotateTrigger(encoder); //} ////5.依次设置曝光高度点位关联的旋转周期和步长 //for (int i = 0; i < m_angleSliceCount; ++i) //{ // int period = m_anglePeriods[i]; // int step = m_angleSteps[i]; // if (period <= effectivePeriod || period > 0xFFFF) // { // TPRINTA_ERROR("Error At Tomo Calculation.idx:%d,Period:%u,Effective:%u", period, effectivePeriod); // continue; // } // m_motorTubeAngle->AppendPWM(step,period); //} ////6.设置曝光触发角度编码器值回传 //auto sensortubeHeightID = m_encoderTubeHeight->GetInterfaceID(ID_COMMUNICATE_INTERFACE); //m_exposure->ActiveExposureTubeHeightPositionAutoNotify(sensortubeHeightID); //auto sensortubeAngleID = m_encoderTubeAngle->GetInterfaceID(ID_COMMUNICATE_INTERFACE); //m_exposure->ActiveExposureTubeAnglePositionAutoNotify(sensortubeAngleID); } BOOL TomoMotionModel::GetTomoRotateDirection() { auto direction = (int)atoi(m_ModelParams["TomoRotateDirection"]); return direction != 0; } void TomoMotionModel::MotionStageClearParams() { m_FeedbackTubeHeightEncoders.clear(); m_FeedbackTubeAngleEncoders.clear(); m_motorTubeAngle->ClearSignal(); m_encoderTubeHeight->ClearTrigger(); if (m_encoderTubeHorizontal) { m_encoderTubeHorizontal->ClearTrigger(); } } void TomoMotionModel::MotionStageCalculateParams() { float detectorHeight = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HEIGHT); float sid = strtof((const char*)m_ModelParams["TomoSID"], nullptr); if (m_CurWS == WS_WALL) { if (m_ModelParams.GetFirstOf("TomoSID_Wall") >= 0) { sid = strtof((const char*)m_ModelParams["TomoSID_Wall"], nullptr); } } else if (m_CurWS == WS_TABLE) { if (m_ModelParams.GetFirstOf("TomoSID_Table") >= 0) { sid = strtof((const char*)m_ModelParams["TomoSID_Table"], nullptr); } } float acc = strtof((const char*)m_ModelParams["TomoAcceleratingDistance"], nullptr); float lat = strtof((const char*)m_ModelParams["TomoAlignmentDistance"], nullptr); float sus = strtof((const char*)m_ModelParams["TomoResetDistance"], nullptr); float brk = strtof((const char*)m_ModelParams["TomoDeceleratingDistance"], nullptr); float anglePerMot = m_coordinates->GetPhysicalPerMotorPulse(TOMO_TUBE_ANGLE); float speed = strtof((const char *)m_TechnicalParams["TomoMotionSpeed"],nullptr); int nproject = (int)m_TechnicalParams["TomoProjectionNumber"]; float scnAngle = strtof((const char *)m_TechnicalParams["TomoScanAngle"],nullptr); memset(m_heightSlices, 0, TOMO_HEIGHT_SLICE_MAX); memset(m_anglePeriods, 0, TOMO_HEIGHT_SLICE_MAX); memset(m_angleSteps, 0, TOMO_HEIGHT_SLICE_MAX); string strScanAngle = (const char*)m_TechnicalParams["TomoScanAngle"]; int nstatus = IsTomoMotionDetectorPosCorrect(detectorHeight, strScanAngle); if (nstatus != 0) { if (gbusinessLog) gbusinessLog->Warn("[TomoMotionModel][MotionStageCalculateParams]->[Current detector position is out of tomo scan range] ]"); m_coordinates->NotifySystemIsTomoMotionLimitation(nstatus); return; } else { if (gbusinessLog) gbusinessLog->Info("[TomoMotionModel][MotionStageCalculateParams]->[Current detector position is in tomo scan range] ]"); m_coordinates->NotifySystemIsTomoMotionLimitation(nstatus); } if(gbusinessLog) gbusinessLog->Info("[TomoMotionModel][MotionStageCalculateParams]->[{$:f3} {$:f3} {$:f3} {$:f3} {$:f3} {$:f3} {$:f3} {$:d} {$:f3} ]", sid, acc, lat, sus, brk, anglePerMot, speed, nproject, scnAngle); auto err = tomoMotionGeometry(true, detectorHeight, acc, brk, lat, sus, sid, speed, nproject, scnAngle, anglePerMot, 1.0, m_warmAngle, m_anglePerSlice, m_heightSlices, m_angleSteps, m_anglePeriods, m_angleSliceCount, m_heightSliceCount); if (err < 0) { if (gmotionLog) gmotionLog->Error("[TomoMotionModel][tomoMotionGeometry]->[Error occurs.]"); } m_heightStartPosition = m_heightSlices[0]; m_angleStartPosition = -(scnAngle / 2.0f); FixHeightStartPosition(speed); //auto period = (DWORD)atoi((const char *)m_MachineryParams["TubeAnglePeriodP0"]); auto period = ConfigurerMotion::GetTubeRotateNormalPeriod(); DigitalTwinLogger::Instance()->ReceiveTomoScanParam(nproject, scnAngle); DigitalTwinLogger::Instance()->ReceiveSID(sid); DigitalTwinLogger::Instance()->ReceiveTomoHeightSpeed(speed); DigitalTwinLogger::Instance()->ReceiveTomoNormalRotatePeriod(period); DigitalTwinLogger::Instance()->ReceiveTomoProjectParams(acc, lat, sus, brk); DigitalTwinLogger::Instance()->ReceiveTomoProjectSliceParams(m_heightSlices, m_heightSliceCount); DigitalTwinLogger::Instance()->ReceiveTomoSliceRotateSpeeds(m_anglePeriods, m_angleSliceCount); DigitalTwinLogger::Instance()->ReceiveTomoSliceRotateSteps(m_angleSteps, m_angleSliceCount); } void TomoMotionModel::DoMotionStageMovetoEndPosHeight() { DigitalTwinLogger::Instance()->ReceiveTimePoint(); auto tubeAngleSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_ANGLE); auto tubeHeightSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_HEIGHT); auto detectorHeight = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HEIGHT); DigitalTwinLogger::Instance()->ReceiveTomoStartPosition(tubeAngleSensorValue, tubeHeightSensorValue); DigitalTwinLogger::Instance()->ReceiveDetectorHeight(detectorHeight); float endPosition = m_heightSlices[m_heightSliceCount - 1]; float currentPosition = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HEIGHT); int direction = JudgeDirectionInTubeHeightAxis(currentPosition, endPosition); MoveTubeHeight(direction, abs(currentPosition - endPosition)); } void TomoMotionModel::DoMotionStageMovetoEndPosHorizontal() { DigitalTwinLogger::Instance()->ReceiveTimePoint(); auto tubeAngleSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_ANGLE); auto tubeHorizontalSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_HORIZONTAL); auto detectorHorizontal = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HORIZONTAL); DigitalTwinLogger::Instance()->ReceiveTomoStartPosition(tubeAngleSensorValue, tubeHorizontalSensorValue); DigitalTwinLogger::Instance()->ReceiveDetectorHeight(detectorHorizontal); float endPosition = m_heightSlices[m_heightSliceCount - 1]; //float currentPosition = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HORIZONTAL); float currentPosition = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HORIZONTAL); int direction = JudgeDirectionInTubeHorizontalAxis(currentPosition, endPosition); MoveTubeHorizontal(direction, abs(currentPosition - endPosition)); } void TomoMotionModel::MotionStageMovetoEndPos() { if (m_CurWS == WS_WALL) { DoMotionStageMovetoEndPosHeight(); } else if (m_CurWS == WS_TABLE) { DoMotionStageMovetoEndPosHorizontal(); } //DigitalTwinLogger::Instance()->ReceiveTimePoint(); //auto tubeAngleSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_ANGLE); //auto tubeHeightSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_HEIGHT); //auto detectorHeight = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HEIGHT); //DigitalTwinLogger::Instance()->ReceiveTomoStartPosition(tubeAngleSensorValue, tubeHeightSensorValue); //DigitalTwinLogger::Instance()->ReceiveDetectorHeight(detectorHeight); //float endPosition = m_heightSlices[m_heightSliceCount - 1]; //float currentPosition = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HEIGHT); //int direction = JudgeDirectionInTubeHeightAxis(currentPosition, endPosition); //MoveTubeHeight(direction, abs(currentPosition - endPosition)); } void TomoMotionModel::DoMotionStageMovetoStartPosHeight() { DigitalTwinLogger::Instance()->ReceiveTimePoint(); auto tubeAngleSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_ANGLE); auto tubeHeightSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_HEIGHT); DigitalTwinLogger::Instance()->ReceiveMotionInitialPosition(tubeAngleSensorValue, tubeHeightSensorValue); MoveTubeHeightToTomoStartPosiotn(); RotateTubeAngleToTomoStartPostion(); } void TomoMotionModel::DoMotionStageMovetoStartPosHorizontal() { DigitalTwinLogger::Instance()->ReceiveTimePoint(); auto tubeAngleSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_ANGLE); auto tubeHorizontalSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_HORIZONTAL); DigitalTwinLogger::Instance()->ReceiveMotionInitialPosition(tubeAngleSensorValue, tubeHorizontalSensorValue); MoveTubeHorizontalToTomoStartPosiotn(); RotateTubeAngleToTomoStartPostion(); } void TomoMotionModel::MotionStageMovetoStartPos() { if (m_CurWS == WS_WALL) { DoMotionStageMovetoStartPosHeight(); } else if (m_CurWS == WS_TABLE) { DoMotionStageMovetoStartPosHorizontal(); } /*DigitalTwinLogger::Instance()->ReceiveTimePoint(); auto tubeAngleSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_ANGLE); auto tubeHeightSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_HEIGHT); DigitalTwinLogger::Instance()->ReceiveMotionInitialPosition(tubeAngleSensorValue, tubeHeightSensorValue); MoveTubeHeightToTomoStartPosiotn(); RotateTubeAngleToTomoStartPostion();*/ } void TomoMotionModel::MoveTubeHeightToTomoStartPosiotn() { float tubeHeight = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HEIGHT); int direction = JudgeDirectionInTubeHeightAxis(tubeHeight, m_heightStartPosition); float speed = 0.0f; if (m_MachineryParams.GetFirstOf("MotionSpeed") > 0) { speed = (float)atof((const char *)m_MachineryParams["MotionSpeed"]); } MoveTubeHeight(direction, abs(m_heightStartPosition - tubeHeight), speed); } void TomoMotionModel::MoveTubeHorizontalToTomoStartPosiotn() { float tubeHorizontal = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HORIZONTAL); int direction = JudgeDirectionInTubeHorizontalAxis(tubeHorizontal, m_heightStartPosition); float speed = 0.0f; if (m_MachineryParams.GetFirstOf("MotionSpeed") > 0) { speed = (float)atof((const char*)m_MachineryParams["MotionSpeed"]); } MoveTubeHorizontal(direction, abs(m_heightStartPosition - tubeHorizontal), speed); } void TomoMotionModel::RotateTubeAngleToTomoStartPostion() { float tubeAngle = m_coordinates->GetCurrentPhysical(TOMO_TUBE_ANGLE); int direction = JudgeDirectionInTubeAngleAxis(tubeAngle, m_angleStartPosition); if(gbusinessLog) gbusinessLog->Info("[TomoMotionModel][RotateTubeAngleToTomoStartPostion]->[{$:f6} {$:f6}]", m_angleStartPosition, tubeAngle); RotateTubeAngle(direction, abs(m_angleStartPosition - tubeAngle)); } void TomoMotionModel::MotionStageErrorStop() { StopMotion(); } void TomoMotionModel::MotionStageErrorRecover() { StopMotion(); } void TomoMotionModel::StopMotion() { m_motorTubeAngle->StopRotation(); m_motorTubeHeight->StopMove(); m_motorTubeHorizontal->StopMove(); auto tubeAngleSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_ANGLE); auto tubeHeightSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_HEIGHT); DigitalTwinLogger::Instance()->ReceiveMotionStopPosition(tubeAngleSensorValue, tubeHeightSensorValue); DigitalTwinLogger::Instance()->SaveMotionRecord(); } void TomoMotionModel::MotionStageCenterAdjust() { AlignTubeHeightToDetector(); RotateTubeToHorizontal(); } void TomoMotionModel::AlignTubeHeightToDetector() { float detectorHeight = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HEIGHT); float tubeHeight = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HEIGHT); int direction = JudgeDirectionInTubeHeightAxis(tubeHeight, detectorHeight); MoveTubeHeight(direction, abs(detectorHeight - tubeHeight)); } void TomoMotionModel::RotateTubeToHorizontal() { float tubeAngle = m_coordinates->GetCurrentPhysical(TOMO_TUBE_ANGLE); int direction = JudgeDirectionInTubeAngleAxis(tubeAngle,0.0f); RotateTubeAngle(direction, tubeAngle); } int TomoMotionModel::JudgeDirectionInTubeHeightAxis(float current, float target) { //auto positive = (int)atoi((const char *)m_MachineryParams["TubeHeightAxisPositiveDirection"]); auto positive = ConfigurerMotion::GetTubeHeightAxisPositiveDirection(); auto direction = positive > 0 ? 1 : -1; if (current > target) { return 1 * direction; } if (current < target) { return -1 * direction; } return 0; } int TomoMotionModel::JudgeDirectionInTubeAngleAxis(float current, float target) { //auto positive = (int)atoi((const char *)m_MachineryParams["TubeRotateAxisPositiveDirection"]); auto positive = ConfigurerMotion::GetTubeRotateAxisPositiveDirection(); auto direction = positive > 0 ? 1 : -1; if (current > target) { return -1 * direction; } if (current < target) { return 1 * direction; } return 0; } void TomoMotionModel::MoveTubeHeight(int direction, float offset, float spd) { auto tolerance = ConfigurerMotion::GetMotionToleranceLine(); if (offset < tolerance) { if (gmotionLog) gmotionLog->Info("[TomoMotionModel][MoveTubeHeight]->[ Offset is {$:f6}, less than {$:f6}m, no motion]", offset, tolerance); return; } float speed = strtof((const char *)m_TechnicalParams["TomoMotionSpeed"], nullptr); auto step = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HEIGHT, offset); auto period = m_coordinates->ConvertMotorSpeed(CONTROLLER_TUBE_HEIGHT, speed); if (period < 50 || (period - 50) < 30) { period = 50; } if (m_tubeLineMotionSwitch) { m_tubeLineMotionSwitch->OutputSignal(false); } m_motorTubeHeight->Move(direction, step, period); } void TomoMotionModel::RotateTubeAngle(int direction, float offset) { //auto period = (DWORD)atoi((const char *)m_MachineryParams["TubeAnglePeriodP0"]); auto period = ConfigurerMotion::GetTubeRotateNormalPeriod(); auto step = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_ANGLE, offset); m_motorTubeAngle->Rotate(direction, step, period); } void TomoMotionModel::UpdateDigitalTwinTomoSliceParams() { DigitalTwinLogger::Instance()->ReceiveTomoProjectSliceParams(m_heightSlices, m_heightSliceCount); } void TomoMotionModel::FixHeightStartPosition(float speed) { //由于曝光系统存在延迟,因此在设置曝光触发点位时需要考虑系统时延 if(gbusinessLog) gbusinessLog->Info("[TomoMotionModel][FixStartPos]->[S][{$:f6} {$:f6} {$:f6}]", m_heightSlices[0], m_heightSlices[1], m_heightSlices[2]); float systemdelayTime = ConfigureExposureDelayTime::GetExposureDelayTimeInSeconds((const char *)m_TechnicalParams["PositionNumber"]); int direct = 1; if (m_heightSlices[0] < m_heightSlices[1]) { direct = -1; } float detaHeight = speed * systemdelayTime * direct; if (((m_heightSlices[2] + detaHeight) - (m_heightSlices[1])) * direct > 0) { m_heightSlices[0] = m_heightSlices[0] + detaHeight; m_heightSlices[1] = m_heightSlices[1] + detaHeight; m_heightStartPosition = m_heightSlices[0]; } if(gbusinessLog) gbusinessLog->Info("[TomoMotionModel][FixStartPos]->[E][{$:f6} {$:f6} {$:f6} [{$:f6}]]", m_heightSlices[0], m_heightSlices[1], m_heightSlices[2], detaHeight); } void TomoMotionModel::SwitchScanningComponents(int nSwitch) { } void TomoMotionModel::SwitchWorkstation(string ws) { if (gmotionLog) gmotionLog->Info("[TomoMotionModel][SwitchWorkstation]->[{$}]", ws.c_str()); m_CurWS = ws; } void TomoMotionModel::OnMotionStageMove2SID(string ws) { if (ws == WS_WALL) { //float desPos = atof((const char*)m_MachineryParams["SID_Wall"]); float desPos = ConfigurerWS::GetDefaultSIDWall(); //float tubeHorizontalPos = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HORIZONTAL); float tubeHorizontalPos = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HORIZONTAL); int direction = JudgeDirectionInTubeHorizontalAxis(tubeHorizontalPos, desPos); if (gmotionLog) gmotionLog->Info("[TomoMotionModel][OnMotionStageMove2SID]->[{$}][Dafault SID:{$:f6}, Cur Pos:{$:f6}]", ws.c_str(), desPos, tubeHorizontalPos); MoveTubeHorizontal(direction, abs(tubeHorizontalPos - desPos)); } else if (ws == WS_TABLE) { //float desPos = atof((const char*)m_MachineryParams["SID_Table"]); float desPos = ConfigurerWS::GetDefaultSIDTable(); float tubeHeightPos = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HEIGHT); int direction = JudgeDirectionInTubeHeightAxis(tubeHeightPos, desPos); if (gmotionLog) gmotionLog->Info("[TomoMotionModel][OnMotionStageMove2SID]->[{$}][Dafault SID:{$:f6}, Cur Pos:{$:f6}]", ws.c_str(), desPos, tubeHeightPos); MoveTubeHeight(direction, abs(tubeHeightPos - desPos)); } } int TomoMotionModel::JudgeDirectionInTubeHorizontalAxis(float current, float target) { //auto positive = (int)atoi((const char*)m_MachineryParams["TubeHorizontalAxisPositiveDirection"]); auto positive = ConfigurerMotion::GetTubeHorizontalAxisPositiveDirection(); auto direction = positive > 0 ? 1 : -1; if (current > target) { return 1 * direction; } if (current < target) { return -1 * direction; } return 0; } void TomoMotionModel::MoveTubeHorizontal(int direction, float offset, float spd) { auto tolerance = ConfigurerMotion::GetMotionToleranceLine(); if (offset < tolerance) { if (gmotionLog) gmotionLog->Info("[TomoMotionModel][MoveTubeHorizontal]->[ Offset is {$:f6}, less than {$:f6}m, no motion]", offset, tolerance); return; } float speed = strtof((const char*)m_TechnicalParams["TomoMotionSpeed"], nullptr); auto step = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HORIZONTAL, offset); auto period = m_coordinates->ConvertMotorSpeed(CONTROLLER_TUBE_HORIZONTAL, speed); if (period < 50 || (period - 50) < 30) { period = 50; } if (m_tubeLineMotionSwitch) { m_tubeLineMotionSwitch->OutputSignal(true); } m_motorTubeHorizontal->Move(direction, step, period); } int TomoMotionModel::IsTomoMotionDetectorPosCorrect(float fDetectorCurPos, string scanangle) { float fDetectorMinPos = 0.0; float fDetectorMaxPos = 0.0; if (gmotionLog) gmotionLog->Info("[TomoMotionModel][IsTomoMotionDetectorPosCorrect]->[ WS is {$}, scan angle is {$}, current detector position is {$:f6}]", m_CurWS, scanangle, fDetectorCurPos); if (m_CurWS == WS_WALL) { fDetectorMinPos = ConfigurerTomoMotionLimitation::GetDetectorMinPositionWall(scanangle); fDetectorMaxPos = ConfigurerTomoMotionLimitation::GetDetectorMaxPositionWall(scanangle); } else if (m_CurWS == WS_TABLE) { fDetectorMinPos = ConfigurerTomoMotionLimitation::GetDetectorMinPositionTable(scanangle); fDetectorMaxPos = ConfigurerTomoMotionLimitation::GetDetectorMaxPositionTable(scanangle); } int nRes = DET_HIGHT_NORMAL; if (fDetectorMaxPos > 0.1 && fDetectorMinPos > 0.1 && fDetectorMaxPos > fDetectorMinPos) { if (fDetectorCurPos > fDetectorMaxPos) { if (gmotionLog) gmotionLog->Warn("[TomoMotionModel][IsTomoMotionDetectorPosCorrect]->[Detector current pos is out of range, max={$:f6}m, current={$:f6}m, no motion]", fDetectorMaxPos, fDetectorCurPos); nRes = DET_HIGHT_TOOHIGH; } else if (fDetectorCurPos < fDetectorMinPos) { if (gmotionLog) gmotionLog->Warn("[TomoMotionModel][IsTomoMotionDetectorPosCorrect]->[Detector current pos is out of range, min={$:f6}m, current={$:f6}m, no motion]", fDetectorMinPos, fDetectorCurPos); nRes = DET_HIGHT_TOOLOW; } } else { if (gmotionLog) gmotionLog->Warn("[TomoMotionModel][IsTomoMotionDetectorPosCorrect]->[Detector limitation pos is not correct max={$:f6}m, min={$:f6}m, no motion]", fDetectorMaxPos, fDetectorMinPos); nRes = DET_HIGHT_TOOHIGH; } return nRes; }