123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819 |
- #include "stdafx.h"
- #include "ResetMotionModel.h"
- #include "IMachineryManager.h"
- #include "MotionStages.h"
- #include "ResetMotionStageArgs.h"
- #include "ITubeAngleController.h"
- #include "ITubeHeightController.h"
- #include "ISensorADController.h"
- #include "ISensorEncoderController.h"
- #include "ILandmarkController.h"
- #include "IPositionManager.h"
- #include "IOInterfaceMapper.h"
- #include "ConfigurerMotion.h"
- #include "TubeLineMotionSwitchController.h"
- #include "ConfigurerWS.h"
- using namespace DIOS::Dev::Detail::MachineryECOM;
- ResetMotionModel::ResetMotionModel()
- :m_StageName(""),
- m_stageArgs(new ResetMotionStageArgs()),
- m_coordinates(nullptr),
- m_motorTubeAngle(nullptr),
- m_motorTubeHeight(nullptr),
- m_adDetectorHeight(nullptr),
- m_encoderTubeAngle(nullptr),
- m_encoderTubeHeight(nullptr),
- m_landmark(nullptr),
- m_resetRotatePeriod(250),
- m_resetHeightPeriod(250),
- m_tubeRotateLandmarkDirection(0),
- m_tubeHeightLandmarkDirection(0),
- m_tubeRotateResetAngle(0.0f),
- m_tubeHeightAxisPositiveDirection(1),
- m_tubeRotateAxisPositiveDirection(1),
- m_tubeHeightLowPositionofRest(0.0),
- m_motorTubeHorizontal(nullptr),
- m_adTubeHorizontal(nullptr),
- m_encoderTubeHorizontal(nullptr),
- m_tubeLineMotionSwitch(nullptr),
- m_tubeHorizontalLandmarkDirection(1),
- m_tubeHorizontalAxisPositiveDirection(1),
- m_resetHorizontalPeriod(500)
- {
- }
- ResetMotionModel::~ResetMotionModel()
- {
- if (m_stageArgs)
- {
- delete m_stageArgs;
- m_stageArgs = nullptr;
- }
- }
- void ResetMotionModel::ChangeStage(const std::string &stageName)
- {
- m_StageName = stageName;
- OnMotionStage(m_StageName);
- }
- std::string ResetMotionModel::GetStageName()
- {
- return m_StageName;
- }
- IMotionStageArgs *ResetMotionModel::GetStageArgs()
- {
- return m_stageArgs;
- }
- void ResetMotionModel::OnMotionStage(const std::string &name)
- {
- if (gmotionLog) gmotionLog->Info("[ResetMotionModel][OnMotionStage]->[Enter][{$}]", name.c_str());
- if (name == RESET_STAGE_CLEAE_PARAMS)
- {
- OnStageClearParams();
- }
- else if (name == RESET_STAGE_TUBE_ANGLE_FIND_LANDMARK)
- {
- OnStageTubeAngleFindLandmark();
- }
- else if (name == RESET_STAGE_TUBE_HEIGHT_FIND_LANDMARK)
- {
- OnStageTubeHeightFindLandmark();
- }
- else if (name == RESET_STAGE_TUBE_ANGLE_MOVE_TO_LANDMARK)
- {
- OnStageTubeAngleMoveToLandmark();
- }
- else if (name == RESET_STAGE_TUBE_HEIGHT_MOVE_TO_LANDMARK)
- {
- OnStageTubeHeightMoveToLandmark();
- }
- else if (name == RESET_STAGE_TUBE_ANGLE_STOP_AT_LANDMARK)
- {
- OnStageTubeAngleStopAtLandmark();
- }
- else if (name == RESET_STAGE_TUBE_HEIGHT_STOP_AT_LANDMARK)
- {
- OnStageTubeHeightStopAtLandmark();
- }
- else if (name == RESET_STAGE_TUBE_ANGLE_ATTACH_ZAXIS)
- {
- OnStageTubeAngleAttachZAxis();
- }
- else if (name == RESET_STAGE_TUBE_HEIGHT_ATTACH_ZAXIS)
- {
- OnStageTubeHeightAttachZAxis();
- }
- else if (name == RESET_STAGE_TUBE_ANGLE_DETACH_ZAXIS)
- {
- OnStageTubeAngleDetachZAxis();
- }
- else if (name == RESET_STAGE_TUBE_HEIGHT_DETACH_ZAXIS)
- {
- OnStageTubeHeightDetachZAxis();
- }
- else if (name == RESET_STAGE_TUBE_ANGLE_FIND_Z_AXIS)
- {
- OnStageTubeAngleFindZAxis();
- }
- else if (name == RESET_STAGE_TUBE_HEIGHT_FIND_Z_AXIS)
- {
- OnStageTubeHeightFindZAxis();
- }
- else if (name == RESET_STAGE_TUBE_ANGLE_STOP_AT_ZAXIS)
- {
- OnStageTubeAngleStopAtZAxis();
- }
- else if (name == RESET_STAGE_TUBE_HEIGHT_STOP_AT_ZAXIS)
- {
- OnStageTubeHeightStopAtZxis();
- }
- else if (name == RESET_STAGE_TUBE_ANGLE_FINISH_RESET)
- {
- OnStageTubeAngleFinishReset();
- }
- else if (name == RESET_STAGE_TUBE_HEIGHT_FINISH_RESET)
- {
- OnStageTubeHeightFinishReset();
- }
- else if (name == RESET_STAGE_STOP_MOVE)
- {
- OnStageStopMove();
- }
- else if (name == RESET_STAGE_TUBE_ANGLE_GO_OUT_OF_LOW_LANDMARK)
- {
- OnStageTubeAngleGoOutOfLowLandmark();
- }
- else if (name == RESET_STAGE_TUBE_ANGLE_GO_OUT_OF_HIGH_LANDMARK)
- {
- OnStageTubeAngleGoOutOfHighLandmark();
- }
- else if (name == RESET_STAGE_TUBE_HEIGHT_GO_OUT_OF_LOW_LANDMARK)
- {
- OnStageTubeHeightGoOutOfLowLandmark();
- }
- else if (name == RESET_STAGE_TUBE_HEIGHT_GO_OUT_OF_HIGH_LANDMARK)
- {
- OnStageTubeHeightGoOutOfHighLandmark();
- }
- else if (name == RESET_STAGE_TUBE_HORIZONTAL_FIND_LANDMARK)
- {
- OnStageTubeHorizontalFindLandmark();
- }
- else if (name == RESET_STAGE_TUBE_HORIZONTAL_MOVE_TO_LANDMARK)
- {
- OnStageTubeHorizontalMoveToLandmark();
- }
- else if (name == RESET_STAGE_TUBE_HORIZONTAL_STOP_AT_LANDMARK)
- {
- OnStageTubeHorizontalStopAtLandmark();
- }
- else if (name == RESET_STAGE_TUBE_HORIZONTAL_ATTACH_ZAXIS)
- {
- OnStageTubeHorizontalAttachZAxis();
- }
- else if (name == RESET_STAGE_TUBE_HORIZONTAL_DETACH_ZAXIS)
- {
- OnStageTubeHorizontalDetachZAxis();
- }
- else if (name == RESET_STAGE_TUBE_HORIZONTAL_FIND_Z_AXIS)
- {
- OnStageTubeHorizontalFindZAxis();
- }
- else if (name == RESET_STAGE_TUBE_HORIZONTAL_STOP_AT_ZAXIS)
- {
- OnStageTubeHorizontalStopAtZxis();
- }
- else if (name == RESET_STAGE_TUBE_HORIZONTAL_FINISH_RESET)
- {
- OnStageTubeHorizontalFinishReset();
- }
- else if (name == RESET_STAGE_TUBE_HORIZONTAL_GO_OUT_OF_LEFT_LANDMARK)
- {
- OnStageTubeHorizontalGoOutOfLeftLandmark();
- }
- else if (name == RESET_STAGE_TUBE_HORIZONTAL_GO_OUT_OF_RIGHT_LANDMARK)
- {
- OnStageTubeHorizontalGoOutOfRightLandmark();
- }
- else if (name == RESET_STAGE_ADJUST_SID_MOVE)
- {
- OnMotionStageMove2SID();
- }
- if (gmotionLog) gmotionLog->Info("[ResetMotionModel][OnMotionStage]->[Exit][{$}]", name.c_str());
- }
- void ResetMotionModel::Initialize(IMachineryManager *machineryManager, IPositionManager *coordinates)
- {
- m_coordinates = coordinates;
- m_motorTubeAngle = (ITubeAngleController *)(machineryManager->Resove(CONTROLLER_TUBE_ANGLE));
- m_motorTubeHeight = (ITubeHeightController *)(machineryManager->Resove(CONTROLLER_TUBE_HEIGHT));
- m_adDetectorHeight = (ISensorADController *)(machineryManager->Resove(CONTROLLER_DETECTOR_HEIGHT_AD));
- m_encoderTubeAngle = (ISensorEncoderController *)(machineryManager->Resove(CONTROLLER_TUBE_ANGLE_ENCODER));
- m_encoderTubeHeight = (ISensorEncoderController *)(machineryManager->Resove(CONTROLLER_TUBE_HEIGHT_ENCODER));
- m_landmark = (ILandmarkController *)(machineryManager->Resove(CONTROLLER_LANDMARK));
- 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_motorTubeHorizontal);
- assert(m_encoderTubeHorizontal);
- assert(m_tubeLineMotionSwitch);
- }
- void ResetMotionModel::LoadMachineryParams(ResDataObject ¶ms)
- {
- m_resetRotatePeriod = (DWORD)ConfigurerMotion::GetTubeRotateResetPeriod();
- m_tubeRotateResetAngle = ConfigurerMotion::GetTubeRotateResetAngle();
- m_tubeRotateAxisPositiveDirection = ConfigurerMotion::GetTubeRotateAxisPositiveDirection();
- m_tubeRotateLandmarkDirection = ConfigurerMotion::GetTubeRotateLandmarkDirection();
- m_resetHeightPeriod = (DWORD)ConfigurerMotion::GetTubeHeightResetPeriod();
- m_tubeHeightLandmarkDirection = ConfigurerMotion::GetTubeHeightLandmarkDirection();
- m_tubeHeightAxisPositiveDirection = ConfigurerMotion::GetTubeHeightAxisPositiveDirection();
- m_resetHorizontalPeriod = (DWORD)ConfigurerMotion::GetTubeHorizontalResetPeriod();
- m_tubeHorizontalLandmarkDirection = ConfigurerMotion::GetTubeHorizontalLandmarkDirection();
- m_tubeHorizontalAxisPositiveDirection = ConfigurerMotion::GetTubeHorizontalAxisPositiveDirection();
- m_tubeHeightLowPositionofRest = ConfigurerMotion::GetTubeHeightLowPositionofReset();
- }
- void ResetMotionModel::LoadModelParams(ResDataObject ¶ms)
- {
- }
- void ResetMotionModel::SetTechnicalParams(ResDataObject ¶ms)
- {
- }
- void ResetMotionModel::OnFeedbackMotionParams(ResDataObject ¶ms)
- {
- }
- void ResetMotionModel::OnStageTubeAngleFindZAxis()
- {
- int direction = ComputeTubeAngleZToLandmarkRotateDirection();
- m_motorTubeAngle->Rotate(direction, -1, m_resetRotatePeriod);
- }
- void ResetMotionModel::OnStageTubeHeightFindZAxis()
- {
- int direction = ComputeTubeHeightZToLandmarkMoveDirection();
- MoveTubeHeight(direction, -1, m_resetHeightPeriod);
- }
- BOOL ResetMotionModel::GetMotionParams(ResDataObject ¶ms)
- {
- return TRUE;
- }
- void ResetMotionModel::OnStageClearParams()
- {
- m_motorTubeAngle->ClearSignal();
- m_encoderTubeHeight->ClearTrigger();
- }
- void ResetMotionModel::OnStageTubeAngleFindLandmark()
- {
- if (IsTubeAngleAtResetLandmark())
- {
- m_stageArgs->IsTubeAngleAtResetLandmark = TRUE;
- return;
- }
- int direction = JudgeTubeRotateLandmarkDirection();
- m_motorTubeAngle->Rotate(direction, -1, m_resetRotatePeriod);
- }
- void ResetMotionModel::OnStageTubeHeightFindLandmark()
- {
- if (IsTubeHeightAtResetLandmark())
- {
- m_stageArgs->IsTubeHeightAtResetLandmark = TRUE;
- return;
- }
- int direction = JudgeTubeHeightLandmarkDirection();
- MoveTubeHeight(direction, -1, m_resetHeightPeriod);
- }
- void ResetMotionModel::OnStageTubeAngleStopAtLandmark()
- {
- m_motorTubeAngle->StopRotation();
- }
- void ResetMotionModel::OnStageTubeHeightStopAtLandmark()
- {
- m_motorTubeHeight->StopMove();
- }
- void ResetMotionModel::OnStageTubeAngleGoOutOfLowLandmark()
- {
- int direction = ComputeTubeAngleZToLandmarkRotateDirection() * (-1);
- m_motorTubeAngle->Rotate(direction, -1, m_resetRotatePeriod);
- }
- void ResetMotionModel::OnStageTubeAngleGoOutOfHighLandmark()
- {
- int direction = ComputeTubeAngleZToLandmarkRotateDirection();
- m_motorTubeAngle->Rotate(direction, -1, m_resetRotatePeriod);
- }
- void ResetMotionModel::OnStageTubeHeightGoOutOfLowLandmark()
- {
- int direction = ComputeTubeHeightZToLandmarkMoveDirection();
- MoveTubeHeight(direction, -1, m_resetHeightPeriod);
- }
- void ResetMotionModel::OnStageTubeHeightGoOutOfHighLandmark()
- {
- int direction = ComputeTubeHeightZToLandmarkMoveDirection();
- MoveTubeHeight(direction, -1, m_resetHeightPeriod);
- }
- void ResetMotionModel::OnStageTubeAngleAttachZAxis()
- {
- m_encoderTubeAngle->BindABAxis();
- m_encoderTubeAngle->BindZAxis();
- m_encoderTubeAngle->ActiveAutoNotifyWhenZClear(true);
- }
- void ResetMotionModel::OnStageTubeHeightAttachZAxis()
- {
- m_encoderTubeHeight->BindABAxis();
- m_encoderTubeHeight->BindZAxis();
- m_encoderTubeHeight->ActiveAutoNotifyWhenZClear(true);
- }
- void ResetMotionModel::OnStageTubeAngleDetachZAxis()
- {
- m_motorTubeAngle->ClearSignal();
- m_encoderTubeAngle->UnBindZAxis();
- m_encoderTubeAngle->ActiveAutoNotifyWhenZClear(false);
- }
- void ResetMotionModel::OnStageTubeHeightDetachZAxis()
- {
- m_encoderTubeHeight->UnBindZAxis();
- m_encoderTubeHeight->ActiveAutoNotifyWhenZClear(false);
- }
- void ResetMotionModel::OnStageTubeAngleFinishReset()
- {
- auto resetAngle = GetTubeRotateResetPosition();
- string ws = ConfigurerWS::GetDefaultWS();
- if (ws == WS_WALL)
- {
- MoveTubeAngle(resetAngle);
- }
- else if (ws == WS_TABLE)
- {
- MoveTubeAngle(-90.0);
- }
- }
- void ResetMotionModel::OnStageTubeHeightFinishReset()
- {
- auto detectorHeight = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HEIGHT);
- if ((m_tubeHeightLowPositionofRest > 0.5 && m_tubeHeightLowPositionofRest < 1.8) && detectorHeight < m_tubeHeightLowPositionofRest)
- {
- if (gbusinessLog) gbusinessLog->Warn("[ResetMotionModel][OnStageTubeHeightFinishReset]->[detector height is lower {$:f6} than lowest position of tube height config {$:f6}, using tube lowest height]", detectorHeight, m_tubeHeightLowPositionofRest);
- detectorHeight = m_tubeHeightLowPositionofRest;
- }
- auto tubeheight = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HEIGHT);
- int direction = JudgeHeightDirection(tubeheight, detectorHeight);
- auto absHeight = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HEIGHT);
- if(gbusinessLog) gbusinessLog->Info("[ResetMotionModel][OnStageTubeHeightFinishReset]->[{$:f6} {$:f6} {$:f6}]", absHeight, detectorHeight, tubeheight);
- auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HEIGHT, abs(tubeheight - detectorHeight));
- MoveTubeHeight(direction, steps, m_resetHeightPeriod);
- }
- void ResetMotionModel::OnStageTubeAngleStopAtZAxis()
- {
- m_motorTubeAngle->StopRotation();
- }
- void ResetMotionModel::OnStageTubeHeightStopAtZxis()
- {
- m_motorTubeHeight->StopMove();
- }
- void ResetMotionModel::OnStageStopMove()
- {
- m_motorTubeAngle->StopRotation();
- m_motorTubeHeight->StopMove();
- }
- void ResetMotionModel::OnStageTubeAngleMoveToLandmark()
- {
- auto currentangle = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_ANGLE);
- auto targetangle = m_coordinates->GetLandmarkPosition(TOMO_TUBE_ANGLE,LANDMARK_LOW);
- int direction = JudgeRotateDirection(currentangle, targetangle);
- auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_ANGLE, abs(currentangle - targetangle));
- m_motorTubeAngle->Rotate(direction, steps, m_resetRotatePeriod);
- }
- void ResetMotionModel::OnStageTubeHeightMoveToLandmark()
- {
- auto currentheight = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HEIGHT);
- auto targetheight = m_coordinates->GetLandmarkPosition(TOMO_TUBE_HEIGHT, LANDMARK_LOW);
-
- if (m_tubeHeightLowPositionofRest > 0.5 && m_tubeHeightLowPositionofRest < 1.8 && targetheight < m_tubeHeightLowPositionofRest)
- {
- if (gbusinessLog) gbusinessLog->Warn("[ResetMotionModel][OnStageTubeHeightMoveToLandmark]->[target height is lower {$:f6} than lowest position of tube height config {$:f6}, using tube lowest height]", targetheight, m_tubeHeightLowPositionofRest);
- targetheight = m_tubeHeightLowPositionofRest;
- }
- int direction = JudgeHeightDirection(currentheight, targetheight);
- auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HEIGHT, abs(currentheight - targetheight));
- if (gbusinessLog) gbusinessLog->Info("[ResetMotionModel][OnStageTubeHeightMoveToLandmark]->[landmark height is {$:f6}, current tube height is {$:f6}]", targetheight, currentheight);
- MoveTubeHeight(direction, steps, m_resetHeightPeriod);
- }
- int ResetMotionModel::JudgeTubeRotateLandmarkDirection()
- {
- int direction = m_tubeRotateAxisPositiveDirection > 0 ? 1 : -1;
- return m_tubeRotateLandmarkDirection > 0 ? direction : (-1 * direction);
- }
- int ResetMotionModel::JudgeTubeHeightLandmarkDirection()
- {
- int direction = m_tubeHeightAxisPositiveDirection > 0 ? 1 : -1;
- return m_tubeHeightLandmarkDirection > 0 ? (-1 * direction) : direction;
- }
- float ResetMotionModel::GetTubeRotateResetPosition()
- {
- return m_tubeRotateResetAngle;
- }
- int ResetMotionModel::JudgeRotateDirection(float currentAngle, float targetAngle)
- {
- int direction = m_tubeRotateAxisPositiveDirection > 0 ? 1 : -1;
- if (currentAngle > targetAngle)
- {
- return -1 * direction;
- }
- if (currentAngle < targetAngle)
- {
- return 1 * direction;
- }
- return 0;
- }
- int ResetMotionModel::JudgeHeightDirection(float currentHeight, float targetHeight)
- {
- int direction = m_tubeHeightAxisPositiveDirection > 0 ? 1 : -1;
- if (currentHeight < targetHeight)
- {
- return -1 * direction;
- }
- if (currentHeight > targetHeight)
- {
- return 1 * direction;
- }
- return 0;
- }
- int ResetMotionModel::ComputeTubeAngleZToLandmarkRotateDirection()
- {
- return -1 * JudgeTubeRotateLandmarkDirection();
- }
- int ResetMotionModel::ComputeTubeHeightZToLandmarkMoveDirection()
- {
- return -1 * JudgeTubeHeightLandmarkDirection();
- }
- BOOL ResetMotionModel::IsTubeAngleAtResetLandmark()
- {
- BOOL ret = FALSE;
- if (m_tubeRotateLandmarkDirection > 0)
- {
- auto highstatus = m_landmark->ReadTubeAngleLowLandmarkStatus();
- ret = (highstatus == 1);
- if (ret)
- {
- m_stageArgs->ActivedTubeAngleLandmark = 1;
- }
- }
- else
- {
- auto lowstatus = m_landmark->ReadTubeAngleHighLandmarkStatus();
- ret = (lowstatus == 1);
- if (ret)
- {
- m_stageArgs->ActivedTubeAngleLandmark = 0;
- }
- }
- return ret;
- }
- BOOL ResetMotionModel::IsTubeHeightAtResetLandmark()
- {
- BOOL ret = FALSE;
- if (m_tubeHeightLandmarkDirection > 0)
- {
- auto highstatus = m_landmark->ReadTubeHeightHighLandmarkStatus();
- ret = (highstatus == 1);
- if (ret)
- {
- m_stageArgs->ActivedTubeHeightLandmark = 1;
- }
- }
- else
- {
- auto lowstatus = m_landmark->ReadTubeHeightLowLandmarkStatus();
- ret = (lowstatus == 1);
- if (ret)
- {
- m_stageArgs->ActivedTubeHeightLandmark = 0;
- }
- }
- return ret;
- }
- void ResetMotionModel::SwitchScanningComponents(int nSwitch)
- {
- }
- void ResetMotionModel::SwitchWorkstation(string ws)
- {
- m_CurWS = ws;
- }
- void ResetMotionModel::OnStageTubeHorizontalMoveToLandmark()
- {
- auto tubehight = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HEIGHT);
- if (tubehight < m_tubeHeightLowPositionofRest)
- {
- if (gbusinessLog) gbusinessLog->Warn("[ResetMotionModel][OnStageTubeHorizontalMoveToLandmark]->[tube height is lower {$:f6} than lowest position of tube height config {$:f6}, no moving]", tubehight, m_tubeHeightLowPositionofRest);
- return;
- }
- auto currenthorizontalpos = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HORIZONTAL);
- auto targetpos = m_coordinates->GetLandmarkPosition(TOMO_TUBE_HORIZONTAL, LANDMARK_LOW);
- int direction = JudgeHorizontalDirection(currenthorizontalpos, targetpos);
- auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HORIZONTAL, abs(currenthorizontalpos - targetpos));
- MoveTubeHorizontal(direction, steps, m_resetHorizontalPeriod);
- }
- void ResetMotionModel::OnStageTubeHorizontalFindLandmark()
- {
- if (IsTubeHorizontalAtResetLandmark())
- {
- m_stageArgs->IsTubeHorizontalAtResetLandmark = TRUE;
- return;
- }
- int direction = JudgeTubeHorizontalLandmarkDirection();
- MoveTubeHorizontal(direction, -1, m_resetHorizontalPeriod);
- }
- void ResetMotionModel::OnStageTubeHorizontalStopAtLandmark()
- {
- if (m_tubeLineMotionSwitch)
- {
- m_tubeLineMotionSwitch->OutputSignal(true);
- }
- if (m_motorTubeHorizontal)
- {
- m_motorTubeHorizontal->StopMove();
- }
- }
- void ResetMotionModel::OnStageTubeHorizontalGoOutOfLeftLandmark()
- {
- int direction = ComputeTubeHorizontalZToLandmarkMoveDirection();
- MoveTubeHorizontal(direction, -1, m_resetHorizontalPeriod);
- }
- void ResetMotionModel::OnStageTubeHorizontalGoOutOfRightLandmark()
- {
- int direction = ComputeTubeHorizontalZToLandmarkMoveDirection();
- MoveTubeHorizontal(direction, -1, m_resetHorizontalPeriod);
- }
- void ResetMotionModel::OnStageTubeHorizontalAttachZAxis()
- {
- if (m_encoderTubeHorizontal)
- {
- m_encoderTubeHorizontal->BindABAxis();
- m_encoderTubeHorizontal->BindZAxis();
- m_encoderTubeHorizontal->ActiveAutoNotifyWhenZClear(true);
- }
- }
- void ResetMotionModel::OnStageTubeHorizontalDetachZAxis()
- {
- if (m_encoderTubeHorizontal)
- {
- m_encoderTubeHorizontal->UnBindZAxis();
- m_encoderTubeHorizontal->ActiveAutoNotifyWhenZClear(false);
- }
- }
- void ResetMotionModel::OnStageTubeHorizontalFindZAxis()
- {
- int direction = ComputeTubeHorizontalZToLandmarkMoveDirection();
- MoveTubeHorizontal(direction, -1, m_resetHorizontalPeriod);
- }
- void ResetMotionModel::OnStageTubeHorizontalStopAtZxis()
- {
- if (m_motorTubeHorizontal)
- {
- m_motorTubeHorizontal->StopMove();
- }
- }
- void ResetMotionModel::OnStageTubeHorizontalFinishReset()
- {
- auto tubehight = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HEIGHT);
- if (tubehight < m_tubeHeightLowPositionofRest)
- {
- if (gbusinessLog) gbusinessLog->Warn("[ResetMotionModel][OnStageTubeHorizontalMoveToLandmark]->[tube height is lower {$:f6} than lowest position of tube height config {$:f6}, no moving]", tubehight, m_tubeHeightLowPositionofRest);
- return;
- }
- auto detectorhorizontal = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HORIZONTAL);
- //auto tubehorizontalpos = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HORIZONTAL);
- auto tubehorizontalpos = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HORIZONTAL);
- auto abshorizontalpos = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HORIZONTAL);
- int direction = JudgeHorizontalDirection(abshorizontalpos, detectorhorizontal);
- if (gbusinessLog) gbusinessLog->Info("[ResetMotionModel][OnStageTubeHorizontalFinishReset]->[{$:f6} {$:f6} {$:f6}]", abshorizontalpos, detectorhorizontal, tubehorizontalpos);
- auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HORIZONTAL, abs(abshorizontalpos - detectorhorizontal));
- MoveTubeHorizontal(direction, steps, m_resetHorizontalPeriod);
- }
- int ResetMotionModel::JudgeTubeHorizontalLandmarkDirection()
- {
- int direction = m_tubeHorizontalAxisPositiveDirection > 0 ? 1 : -1;
- return m_tubeHorizontalLandmarkDirection > 0 ? (-1 * direction) : direction;
- }
- int ResetMotionModel::JudgeHorizontalDirection(float currentHorizontalPos, float targetHorizontalPos)
- {
- int direction = m_tubeHorizontalAxisPositiveDirection > 0 ? 1 : -1;
- if (currentHorizontalPos < targetHorizontalPos)
- {
- return -1 * direction;
- }
- if (currentHorizontalPos > targetHorizontalPos)
- {
- return 1 * direction;
- }
- return 0;
- }
- int ResetMotionModel::ComputeTubeHorizontalZToLandmarkMoveDirection()
- {
- return -1 * JudgeTubeHorizontalLandmarkDirection();
- }
- BOOL ResetMotionModel::IsTubeHorizontalAtResetLandmark()
- {
- BOOL ret = FALSE;
- if (m_tubeHorizontalLandmarkDirection > 0)
- {
- auto rightstatus = m_landmark->ReadTubeHorizontalRightLandmarkStatus();
- ret = (rightstatus == 1);
- if (ret)
- {
- m_stageArgs->ActivedTubeHorizontalLandmark = 1;
- }
- }
- else
- {
- auto leftstatus = m_landmark->ReadTubeHorizontalLeftLandmarkStatus();
- ret = (leftstatus == 1);
- if (ret)
- {
- m_stageArgs->ActivedTubeHorizontalLandmark = 0;
- }
- }
- return ret;
- }
- void ResetMotionModel::MoveTubeHorizontal(int direction, int steps, int pwmperiod)
- {
- if (m_tubeLineMotionSwitch)
- {
- m_tubeLineMotionSwitch->OutputSignal(true);
- }
- if (m_motorTubeHorizontal)
- {
- m_motorTubeHorizontal->Move(direction, steps, pwmperiod);
- }
- }
- void ResetMotionModel::MoveTubeHeight(int direction, int steps, int pwmperiod)
- {
- if (m_tubeLineMotionSwitch)
- {
- m_tubeLineMotionSwitch->OutputSignal(false);
- }
- if (m_motorTubeHeight)
- {
- m_motorTubeHeight->Move(direction, steps, pwmperiod);
- }
- }
- void ResetMotionModel::MoveTubeAngle(float despos)
- {
- auto tubeangle = m_coordinates->GetCurrentPhysical(TOMO_TUBE_ANGLE);
- int direction = JudgeRotateDirection(tubeangle, despos);
- auto absAngle = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_ANGLE);
- if (gbusinessLog) gbusinessLog->Info("[ResetMotionModel][MoveTubeAngle]->[{$:f6} {$:f6} {$:f6}]", absAngle, despos, tubeangle);
- auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_ANGLE, abs(tubeangle - despos));
- m_motorTubeAngle->Rotate(direction, steps, m_resetRotatePeriod);
- }
- void ResetMotionModel::OnMotionStageMove2SID()
- {
- string ws = ConfigurerWS::GetDefaultWS();
- if (gmotionLog) gmotionLog->Info("[ResetMotionModel][OnMotionStageMove2SID]->[{$}]", ws.c_str());
- if (ws == WS_WALL)
- {
- float desPos = ConfigurerWS::GetDefaultSIDWall();
- float tubeHorizontalPos = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HORIZONTAL);
- int direction = JudgeHorizontalDirection(tubeHorizontalPos, desPos);
- if (gmotionLog) gmotionLog->Info("[ResetMotionModel][OnMotionStageMove2SID]->[MoveTubeHorizontal Des pos {$:f6}, Cur pos {$:f6}]", desPos, tubeHorizontalPos);
-
- auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HORIZONTAL, abs(tubeHorizontalPos - desPos));
- MoveTubeHorizontal(direction, steps, m_resetHorizontalPeriod);
- }
- else if (ws == WS_TABLE)
- {
- float desPos = ConfigurerWS::GetDefaultSIDTable();
- float tubeHeightPos = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HEIGHT);
- int direction = JudgeHeightDirection(tubeHeightPos, desPos);
- if (gmotionLog) gmotionLog->Info("[ResetMotionModel][OnMotionStageMove2SID]->[MoveTubeHeight Des pos {$:f6}, Cur pos {$:f6}]", desPos, tubeHeightPos);
- auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HEIGHT, abs(tubeHeightPos - desPos));
- MoveTubeHeight(direction, steps, m_resetHeightPeriod);
- //MoveTubeAngle(-90.0);
- }
- }
|