#include "stdafx.h" #include "ParkingModel.h" #include "MotionStages.h" #include "ParkingMotionStageArgs.h" #include "IMachineryManager.h" #include "ITubeAngleController.h" #include "ITubeHeightController.h" #include "IPositionManager.h" using namespace DIOS::Dev::Detail::MachineryECOM; ParkingModel::ParkingModel() :m_stageArgs(new ParkingMotionStageArgs()), m_coordinates(nullptr), m_motorTubeAngle(nullptr), m_motorTubeHeight(nullptr) { } ParkingModel::~ParkingModel() { if (m_stageArgs) { delete m_stageArgs; m_stageArgs = nullptr; } } std::string ParkingModel::GetStageName() { return m_stageName; } IMotionStageArgs *ParkingModel::GetStageArgs() { return m_stageArgs; } void ParkingModel::ChangeStage(const std::string &stageName) { m_stageName = stageName; OnMotionStage(stageName); } void ParkingModel::Initialize(IMachineryManager *machineryManager, IPositionManager *coordinates) { m_coordinates = coordinates; m_motorTubeAngle = (ITubeAngleController *)(machineryManager->Resove(CONTROLLER_TUBE_ANGLE)); m_motorTubeHeight = (ITubeHeightController *)(machineryManager->Resove(CONTROLLER_TUBE_HEIGHT)); assert(m_motorTubeAngle); assert(m_motorTubeHeight); } void ParkingModel::LoadMachineryParams(ResDataObject ¶ms) { m_MachineryParams = params; } void ParkingModel::LoadModelParams(ResDataObject ¶ms) { } void ParkingModel::SetTechnicalParams(ResDataObject ¶ms) { } void ParkingModel::OnFeedbackMotionParams(ResDataObject ¶ms) { } BOOL ParkingModel::GetMotionParams(ResDataObject ¶ms) { return TRUE; } void ParkingModel::OnMotionStage(const std::string &name) { if (name == PARKING_STAGE_PARK_TO_HORIZONTAL) { OnMotionStageParkingHorizontal(); } else if (name == PARKING_STAGE_PARK_TO_VERTICAL) { OnMotionStageParkingVertical(); } else if (name == PARKING_STAGE_PARK_TO_TUBE_ANGLE_LOW_LIMIT) { OnMotionStageParkingTubeAngleLowLimit(); } else if (name == PARKING_STAGE_PARK_TO_TUBE_HEIGHT_LOW_LIMIT) { OnMotionStageParkingTubeHeightLowLimit(); } else if (name == PARKING_STAGE_STOP_MOVE) { OnMotionStageStopMove(); } } void ParkingModel::OnMotionStageParkingHorizontal() { AlignTubeHeightToDetector(); RotateTubeToExposure(); } void ParkingModel::OnMotionStageParkingVertical() { RotateTubeToVertical(); } void ParkingModel::OnMotionStageParkingTubeAngleLowLimit() { RotateTubeToLowLimit(); } void ParkingModel::OnMotionStageParkingTubeHeightLowLimit() { MoveTubeHeightToLowLimit(); } void ParkingModel::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 ParkingModel::RotateTubeToExposure() { float tubeAngle = m_coordinates->GetCurrentPhysical(TOMO_TUBE_ANGLE); float exposureAngle = GetExposureAngle(); int direction = JudgeDirectionInTubeAngleAxis(tubeAngle, exposureAngle); RotateTubeAngle(direction, tubeAngle); } void ParkingModel::RotateTubeToVertical() { float tubeAngle = m_coordinates->GetCurrentPhysical(TOMO_TUBE_ANGLE); float verticalAngle = GetVerticalAngle(); int direction = JudgeDirectionInTubeAngleAxis(tubeAngle, verticalAngle); RotateTubeAngle(direction, tubeAngle); } void ParkingModel::RotateTubeToLowLimit() { auto period = (DWORD)atoi((const char *)m_MachineryParams["TubeAnglePeriodP0"]); auto currentangle = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_ANGLE); auto targetangle = m_coordinates->GetLandmarkPosition(TOMO_TUBE_ANGLE,LANDMARK_LOW); int direction = JudgeDirectionInTubeAngleAxis(currentangle, targetangle); if(gbusinessLog) gbusinessLog->Info("[ParkingModel][RotateTubeToLowLimit]->[{$:f6} {$:f6}]", currentangle, targetangle); auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_ANGLE, abs(currentangle - targetangle)); m_motorTubeAngle->Rotate(direction, steps, period); } void ParkingModel::MoveTubeHeightToLowLimit() { float speed = (float)atof((const char *)m_MachineryParams["MotionSpeed"]); auto currentheight = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HEIGHT); auto targetheight = m_coordinates->GetLandmarkPosition(TOMO_TUBE_HEIGHT,LANDMARK_LOW); int direction = JudgeDirectionInTubeHeightAxis(currentheight, targetheight); auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HEIGHT, abs(currentheight - targetheight)); auto period = m_coordinates->ConvertMotorSpeed(CONTROLLER_TUBE_HEIGHT, speed); if (period < 50 || (period - 50) < 30) { period = 50; } m_motorTubeHeight->Move(direction, steps, period); } void ParkingModel::MoveTubeHeight(int direction, float offset) { float speed = (float)atof((const char *)m_MachineryParams["MotionSpeed"]); 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; } m_motorTubeHeight->Move(direction, step, period); } void ParkingModel::RotateTubeAngle(int direction, float offset) { auto period = (DWORD)atoi((const char *)m_MachineryParams["TubeAnglePeriodP0"]); auto step = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_ANGLE, offset); m_motorTubeAngle->Rotate(direction, step, period); } float ParkingModel::GetExposureAngle() { return (float)atof((const char *)m_MachineryParams["TubeAngleOfRAD"]); } float ParkingModel::GetVerticalAngle() { return (float)atof((const char *)m_MachineryParams["TubeAngleOfVertical"]); } int ParkingModel::JudgeDirectionInTubeHeightAxis(float current, float target) { auto positive = (int)atoi((const char *)m_MachineryParams["TubeHeightAxisPositiveDirection"]); auto direction = positive > 0 ? 1 : -1; if (current > target) { return 1 * direction; } if (current < target) { return -1 * direction; } return 0; } int ParkingModel::JudgeDirectionInTubeAngleAxis(float current, float target) { auto positive = (int)atoi((const char *)m_MachineryParams["TubeRotateAxisPositiveDirection"]); auto direction = positive > 0 ? 1 : -1; if (current > target) { return -1 * direction; } if (current < target) { return 1 * direction; } return 0; } void ParkingModel::OnMotionStageStopMove() { m_motorTubeAngle->StopRotation(); m_motorTubeHeight->StopMove(); } void ParkingModel::SwitchScanningComponents(int nSwitch) { } void ParkingModel::SwitchWorkstation(string ws) { m_CurWS = ws; }