ResetMotionModel.cpp 24 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819
  1. #include "stdafx.h"
  2. #include "ResetMotionModel.h"
  3. #include "IMachineryManager.h"
  4. #include "MotionStages.h"
  5. #include "ResetMotionStageArgs.h"
  6. #include "ITubeAngleController.h"
  7. #include "ITubeHeightController.h"
  8. #include "ISensorADController.h"
  9. #include "ISensorEncoderController.h"
  10. #include "ILandmarkController.h"
  11. #include "IPositionManager.h"
  12. #include "IOInterfaceMapper.h"
  13. #include "ConfigurerMotion.h"
  14. #include "TubeLineMotionSwitchController.h"
  15. #include "ConfigurerWS.h"
  16. using namespace DIOS::Dev::Detail::MachineryECOM;
  17. ResetMotionModel::ResetMotionModel()
  18. :m_StageName(""),
  19. m_stageArgs(new ResetMotionStageArgs()),
  20. m_coordinates(nullptr),
  21. m_motorTubeAngle(nullptr),
  22. m_motorTubeHeight(nullptr),
  23. m_adDetectorHeight(nullptr),
  24. m_encoderTubeAngle(nullptr),
  25. m_encoderTubeHeight(nullptr),
  26. m_landmark(nullptr),
  27. m_resetRotatePeriod(250),
  28. m_resetHeightPeriod(250),
  29. m_tubeRotateLandmarkDirection(0),
  30. m_tubeHeightLandmarkDirection(0),
  31. m_tubeRotateResetAngle(0.0f),
  32. m_tubeHeightAxisPositiveDirection(1),
  33. m_tubeRotateAxisPositiveDirection(1),
  34. m_tubeHeightLowPositionofRest(0.0),
  35. m_motorTubeHorizontal(nullptr),
  36. m_adTubeHorizontal(nullptr),
  37. m_encoderTubeHorizontal(nullptr),
  38. m_tubeLineMotionSwitch(nullptr),
  39. m_tubeHorizontalLandmarkDirection(1),
  40. m_tubeHorizontalAxisPositiveDirection(1),
  41. m_resetHorizontalPeriod(500)
  42. {
  43. }
  44. ResetMotionModel::~ResetMotionModel()
  45. {
  46. if (m_stageArgs)
  47. {
  48. delete m_stageArgs;
  49. m_stageArgs = nullptr;
  50. }
  51. }
  52. void ResetMotionModel::ChangeStage(const std::string &stageName)
  53. {
  54. m_StageName = stageName;
  55. OnMotionStage(m_StageName);
  56. }
  57. std::string ResetMotionModel::GetStageName()
  58. {
  59. return m_StageName;
  60. }
  61. IMotionStageArgs *ResetMotionModel::GetStageArgs()
  62. {
  63. return m_stageArgs;
  64. }
  65. void ResetMotionModel::OnMotionStage(const std::string &name)
  66. {
  67. if (gmotionLog) gmotionLog->Info("[ResetMotionModel][OnMotionStage]->[Enter][{$}]", name.c_str());
  68. if (name == RESET_STAGE_CLEAE_PARAMS)
  69. {
  70. OnStageClearParams();
  71. }
  72. else if (name == RESET_STAGE_TUBE_ANGLE_FIND_LANDMARK)
  73. {
  74. OnStageTubeAngleFindLandmark();
  75. }
  76. else if (name == RESET_STAGE_TUBE_HEIGHT_FIND_LANDMARK)
  77. {
  78. OnStageTubeHeightFindLandmark();
  79. }
  80. else if (name == RESET_STAGE_TUBE_ANGLE_MOVE_TO_LANDMARK)
  81. {
  82. OnStageTubeAngleMoveToLandmark();
  83. }
  84. else if (name == RESET_STAGE_TUBE_HEIGHT_MOVE_TO_LANDMARK)
  85. {
  86. OnStageTubeHeightMoveToLandmark();
  87. }
  88. else if (name == RESET_STAGE_TUBE_ANGLE_STOP_AT_LANDMARK)
  89. {
  90. OnStageTubeAngleStopAtLandmark();
  91. }
  92. else if (name == RESET_STAGE_TUBE_HEIGHT_STOP_AT_LANDMARK)
  93. {
  94. OnStageTubeHeightStopAtLandmark();
  95. }
  96. else if (name == RESET_STAGE_TUBE_ANGLE_ATTACH_ZAXIS)
  97. {
  98. OnStageTubeAngleAttachZAxis();
  99. }
  100. else if (name == RESET_STAGE_TUBE_HEIGHT_ATTACH_ZAXIS)
  101. {
  102. OnStageTubeHeightAttachZAxis();
  103. }
  104. else if (name == RESET_STAGE_TUBE_ANGLE_DETACH_ZAXIS)
  105. {
  106. OnStageTubeAngleDetachZAxis();
  107. }
  108. else if (name == RESET_STAGE_TUBE_HEIGHT_DETACH_ZAXIS)
  109. {
  110. OnStageTubeHeightDetachZAxis();
  111. }
  112. else if (name == RESET_STAGE_TUBE_ANGLE_FIND_Z_AXIS)
  113. {
  114. OnStageTubeAngleFindZAxis();
  115. }
  116. else if (name == RESET_STAGE_TUBE_HEIGHT_FIND_Z_AXIS)
  117. {
  118. OnStageTubeHeightFindZAxis();
  119. }
  120. else if (name == RESET_STAGE_TUBE_ANGLE_STOP_AT_ZAXIS)
  121. {
  122. OnStageTubeAngleStopAtZAxis();
  123. }
  124. else if (name == RESET_STAGE_TUBE_HEIGHT_STOP_AT_ZAXIS)
  125. {
  126. OnStageTubeHeightStopAtZxis();
  127. }
  128. else if (name == RESET_STAGE_TUBE_ANGLE_FINISH_RESET)
  129. {
  130. OnStageTubeAngleFinishReset();
  131. }
  132. else if (name == RESET_STAGE_TUBE_HEIGHT_FINISH_RESET)
  133. {
  134. OnStageTubeHeightFinishReset();
  135. }
  136. else if (name == RESET_STAGE_STOP_MOVE)
  137. {
  138. OnStageStopMove();
  139. }
  140. else if (name == RESET_STAGE_TUBE_ANGLE_GO_OUT_OF_LOW_LANDMARK)
  141. {
  142. OnStageTubeAngleGoOutOfLowLandmark();
  143. }
  144. else if (name == RESET_STAGE_TUBE_ANGLE_GO_OUT_OF_HIGH_LANDMARK)
  145. {
  146. OnStageTubeAngleGoOutOfHighLandmark();
  147. }
  148. else if (name == RESET_STAGE_TUBE_HEIGHT_GO_OUT_OF_LOW_LANDMARK)
  149. {
  150. OnStageTubeHeightGoOutOfLowLandmark();
  151. }
  152. else if (name == RESET_STAGE_TUBE_HEIGHT_GO_OUT_OF_HIGH_LANDMARK)
  153. {
  154. OnStageTubeHeightGoOutOfHighLandmark();
  155. }
  156. else if (name == RESET_STAGE_TUBE_HORIZONTAL_FIND_LANDMARK)
  157. {
  158. OnStageTubeHorizontalFindLandmark();
  159. }
  160. else if (name == RESET_STAGE_TUBE_HORIZONTAL_MOVE_TO_LANDMARK)
  161. {
  162. OnStageTubeHorizontalMoveToLandmark();
  163. }
  164. else if (name == RESET_STAGE_TUBE_HORIZONTAL_STOP_AT_LANDMARK)
  165. {
  166. OnStageTubeHorizontalStopAtLandmark();
  167. }
  168. else if (name == RESET_STAGE_TUBE_HORIZONTAL_ATTACH_ZAXIS)
  169. {
  170. OnStageTubeHorizontalAttachZAxis();
  171. }
  172. else if (name == RESET_STAGE_TUBE_HORIZONTAL_DETACH_ZAXIS)
  173. {
  174. OnStageTubeHorizontalDetachZAxis();
  175. }
  176. else if (name == RESET_STAGE_TUBE_HORIZONTAL_FIND_Z_AXIS)
  177. {
  178. OnStageTubeHorizontalFindZAxis();
  179. }
  180. else if (name == RESET_STAGE_TUBE_HORIZONTAL_STOP_AT_ZAXIS)
  181. {
  182. OnStageTubeHorizontalStopAtZxis();
  183. }
  184. else if (name == RESET_STAGE_TUBE_HORIZONTAL_FINISH_RESET)
  185. {
  186. OnStageTubeHorizontalFinishReset();
  187. }
  188. else if (name == RESET_STAGE_TUBE_HORIZONTAL_GO_OUT_OF_LEFT_LANDMARK)
  189. {
  190. OnStageTubeHorizontalGoOutOfLeftLandmark();
  191. }
  192. else if (name == RESET_STAGE_TUBE_HORIZONTAL_GO_OUT_OF_RIGHT_LANDMARK)
  193. {
  194. OnStageTubeHorizontalGoOutOfRightLandmark();
  195. }
  196. else if (name == RESET_STAGE_ADJUST_SID_MOVE)
  197. {
  198. OnMotionStageMove2SID();
  199. }
  200. if (gmotionLog) gmotionLog->Info("[ResetMotionModel][OnMotionStage]->[Exit][{$}]", name.c_str());
  201. }
  202. void ResetMotionModel::Initialize(IMachineryManager *machineryManager, IPositionManager *coordinates)
  203. {
  204. m_coordinates = coordinates;
  205. m_motorTubeAngle = (ITubeAngleController *)(machineryManager->Resove(CONTROLLER_TUBE_ANGLE));
  206. m_motorTubeHeight = (ITubeHeightController *)(machineryManager->Resove(CONTROLLER_TUBE_HEIGHT));
  207. m_adDetectorHeight = (ISensorADController *)(machineryManager->Resove(CONTROLLER_DETECTOR_HEIGHT_AD));
  208. m_encoderTubeAngle = (ISensorEncoderController *)(machineryManager->Resove(CONTROLLER_TUBE_ANGLE_ENCODER));
  209. m_encoderTubeHeight = (ISensorEncoderController *)(machineryManager->Resove(CONTROLLER_TUBE_HEIGHT_ENCODER));
  210. m_landmark = (ILandmarkController *)(machineryManager->Resove(CONTROLLER_LANDMARK));
  211. m_motorTubeHorizontal = (ITubeHeightController*)(machineryManager->Resove(CONTROLLER_TUBE_HORIZONTAL));
  212. m_encoderTubeHorizontal = (ISensorEncoderController*)(machineryManager->Resove(CONTROLLER_TUBE_HORIZONTAL_ENCODER));
  213. m_tubeLineMotionSwitch = (IOutputController*)(machineryManager->Resove(CONTROLLER_TUBE_LINE_MOTION_SWTICH));
  214. assert(m_motorTubeAngle);
  215. assert(m_motorTubeHeight);
  216. assert(m_adDetectorHeight);
  217. assert(m_encoderTubeAngle);
  218. assert(m_encoderTubeHeight);
  219. assert(m_motorTubeHorizontal);
  220. assert(m_encoderTubeHorizontal);
  221. assert(m_tubeLineMotionSwitch);
  222. }
  223. void ResetMotionModel::LoadMachineryParams(ResDataObject &params)
  224. {
  225. m_resetRotatePeriod = (DWORD)ConfigurerMotion::GetTubeRotateResetPeriod();
  226. m_tubeRotateResetAngle = ConfigurerMotion::GetTubeRotateResetAngle();
  227. m_tubeRotateAxisPositiveDirection = ConfigurerMotion::GetTubeRotateAxisPositiveDirection();
  228. m_tubeRotateLandmarkDirection = ConfigurerMotion::GetTubeRotateLandmarkDirection();
  229. m_resetHeightPeriod = (DWORD)ConfigurerMotion::GetTubeHeightResetPeriod();
  230. m_tubeHeightLandmarkDirection = ConfigurerMotion::GetTubeHeightLandmarkDirection();
  231. m_tubeHeightAxisPositiveDirection = ConfigurerMotion::GetTubeHeightAxisPositiveDirection();
  232. m_resetHorizontalPeriod = (DWORD)ConfigurerMotion::GetTubeHorizontalResetPeriod();
  233. m_tubeHorizontalLandmarkDirection = ConfigurerMotion::GetTubeHorizontalLandmarkDirection();
  234. m_tubeHorizontalAxisPositiveDirection = ConfigurerMotion::GetTubeHorizontalAxisPositiveDirection();
  235. m_tubeHeightLowPositionofRest = ConfigurerMotion::GetTubeHeightLowPositionofReset();
  236. }
  237. void ResetMotionModel::LoadModelParams(ResDataObject &params)
  238. {
  239. }
  240. void ResetMotionModel::SetTechnicalParams(ResDataObject &params)
  241. {
  242. }
  243. void ResetMotionModel::OnFeedbackMotionParams(ResDataObject &params)
  244. {
  245. }
  246. void ResetMotionModel::OnStageTubeAngleFindZAxis()
  247. {
  248. int direction = ComputeTubeAngleZToLandmarkRotateDirection();
  249. m_motorTubeAngle->Rotate(direction, -1, m_resetRotatePeriod);
  250. }
  251. void ResetMotionModel::OnStageTubeHeightFindZAxis()
  252. {
  253. int direction = ComputeTubeHeightZToLandmarkMoveDirection();
  254. MoveTubeHeight(direction, -1, m_resetHeightPeriod);
  255. }
  256. BOOL ResetMotionModel::GetMotionParams(ResDataObject &params)
  257. {
  258. return TRUE;
  259. }
  260. void ResetMotionModel::OnStageClearParams()
  261. {
  262. m_motorTubeAngle->ClearSignal();
  263. m_encoderTubeHeight->ClearTrigger();
  264. }
  265. void ResetMotionModel::OnStageTubeAngleFindLandmark()
  266. {
  267. if (IsTubeAngleAtResetLandmark())
  268. {
  269. m_stageArgs->IsTubeAngleAtResetLandmark = TRUE;
  270. return;
  271. }
  272. int direction = JudgeTubeRotateLandmarkDirection();
  273. m_motorTubeAngle->Rotate(direction, -1, m_resetRotatePeriod);
  274. }
  275. void ResetMotionModel::OnStageTubeHeightFindLandmark()
  276. {
  277. if (IsTubeHeightAtResetLandmark())
  278. {
  279. m_stageArgs->IsTubeHeightAtResetLandmark = TRUE;
  280. return;
  281. }
  282. int direction = JudgeTubeHeightLandmarkDirection();
  283. MoveTubeHeight(direction, -1, m_resetHeightPeriod);
  284. }
  285. void ResetMotionModel::OnStageTubeAngleStopAtLandmark()
  286. {
  287. m_motorTubeAngle->StopRotation();
  288. }
  289. void ResetMotionModel::OnStageTubeHeightStopAtLandmark()
  290. {
  291. m_motorTubeHeight->StopMove();
  292. }
  293. void ResetMotionModel::OnStageTubeAngleGoOutOfLowLandmark()
  294. {
  295. int direction = ComputeTubeAngleZToLandmarkRotateDirection() * (-1);
  296. m_motorTubeAngle->Rotate(direction, -1, m_resetRotatePeriod);
  297. }
  298. void ResetMotionModel::OnStageTubeAngleGoOutOfHighLandmark()
  299. {
  300. int direction = ComputeTubeAngleZToLandmarkRotateDirection();
  301. m_motorTubeAngle->Rotate(direction, -1, m_resetRotatePeriod);
  302. }
  303. void ResetMotionModel::OnStageTubeHeightGoOutOfLowLandmark()
  304. {
  305. int direction = ComputeTubeHeightZToLandmarkMoveDirection();
  306. MoveTubeHeight(direction, -1, m_resetHeightPeriod);
  307. }
  308. void ResetMotionModel::OnStageTubeHeightGoOutOfHighLandmark()
  309. {
  310. int direction = ComputeTubeHeightZToLandmarkMoveDirection();
  311. MoveTubeHeight(direction, -1, m_resetHeightPeriod);
  312. }
  313. void ResetMotionModel::OnStageTubeAngleAttachZAxis()
  314. {
  315. m_encoderTubeAngle->BindABAxis();
  316. m_encoderTubeAngle->BindZAxis();
  317. m_encoderTubeAngle->ActiveAutoNotifyWhenZClear(true);
  318. }
  319. void ResetMotionModel::OnStageTubeHeightAttachZAxis()
  320. {
  321. m_encoderTubeHeight->BindABAxis();
  322. m_encoderTubeHeight->BindZAxis();
  323. m_encoderTubeHeight->ActiveAutoNotifyWhenZClear(true);
  324. }
  325. void ResetMotionModel::OnStageTubeAngleDetachZAxis()
  326. {
  327. m_motorTubeAngle->ClearSignal();
  328. m_encoderTubeAngle->UnBindZAxis();
  329. m_encoderTubeAngle->ActiveAutoNotifyWhenZClear(false);
  330. }
  331. void ResetMotionModel::OnStageTubeHeightDetachZAxis()
  332. {
  333. m_encoderTubeHeight->UnBindZAxis();
  334. m_encoderTubeHeight->ActiveAutoNotifyWhenZClear(false);
  335. }
  336. void ResetMotionModel::OnStageTubeAngleFinishReset()
  337. {
  338. auto resetAngle = GetTubeRotateResetPosition();
  339. string ws = ConfigurerWS::GetDefaultWS();
  340. if (ws == WS_WALL)
  341. {
  342. MoveTubeAngle(resetAngle);
  343. }
  344. else if (ws == WS_TABLE)
  345. {
  346. MoveTubeAngle(-90.0);
  347. }
  348. }
  349. void ResetMotionModel::OnStageTubeHeightFinishReset()
  350. {
  351. auto detectorHeight = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HEIGHT);
  352. if ((m_tubeHeightLowPositionofRest > 0.5 && m_tubeHeightLowPositionofRest < 1.8) && detectorHeight < m_tubeHeightLowPositionofRest)
  353. {
  354. 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);
  355. detectorHeight = m_tubeHeightLowPositionofRest;
  356. }
  357. auto tubeheight = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HEIGHT);
  358. int direction = JudgeHeightDirection(tubeheight, detectorHeight);
  359. auto absHeight = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HEIGHT);
  360. if(gbusinessLog) gbusinessLog->Info("[ResetMotionModel][OnStageTubeHeightFinishReset]->[{$:f6} {$:f6} {$:f6}]", absHeight, detectorHeight, tubeheight);
  361. auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HEIGHT, abs(tubeheight - detectorHeight));
  362. MoveTubeHeight(direction, steps, m_resetHeightPeriod);
  363. }
  364. void ResetMotionModel::OnStageTubeAngleStopAtZAxis()
  365. {
  366. m_motorTubeAngle->StopRotation();
  367. }
  368. void ResetMotionModel::OnStageTubeHeightStopAtZxis()
  369. {
  370. m_motorTubeHeight->StopMove();
  371. }
  372. void ResetMotionModel::OnStageStopMove()
  373. {
  374. m_motorTubeAngle->StopRotation();
  375. m_motorTubeHeight->StopMove();
  376. }
  377. void ResetMotionModel::OnStageTubeAngleMoveToLandmark()
  378. {
  379. auto currentangle = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_ANGLE);
  380. auto targetangle = m_coordinates->GetLandmarkPosition(TOMO_TUBE_ANGLE,LANDMARK_LOW);
  381. int direction = JudgeRotateDirection(currentangle, targetangle);
  382. auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_ANGLE, abs(currentangle - targetangle));
  383. m_motorTubeAngle->Rotate(direction, steps, m_resetRotatePeriod);
  384. }
  385. void ResetMotionModel::OnStageTubeHeightMoveToLandmark()
  386. {
  387. auto currentheight = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HEIGHT);
  388. auto targetheight = m_coordinates->GetLandmarkPosition(TOMO_TUBE_HEIGHT, LANDMARK_LOW);
  389. if (m_tubeHeightLowPositionofRest > 0.5 && m_tubeHeightLowPositionofRest < 1.8 && targetheight < m_tubeHeightLowPositionofRest)
  390. {
  391. 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);
  392. targetheight = m_tubeHeightLowPositionofRest;
  393. }
  394. int direction = JudgeHeightDirection(currentheight, targetheight);
  395. auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HEIGHT, abs(currentheight - targetheight));
  396. if (gbusinessLog) gbusinessLog->Info("[ResetMotionModel][OnStageTubeHeightMoveToLandmark]->[landmark height is {$:f6}, current tube height is {$:f6}]", targetheight, currentheight);
  397. MoveTubeHeight(direction, steps, m_resetHeightPeriod);
  398. }
  399. int ResetMotionModel::JudgeTubeRotateLandmarkDirection()
  400. {
  401. int direction = m_tubeRotateAxisPositiveDirection > 0 ? 1 : -1;
  402. return m_tubeRotateLandmarkDirection > 0 ? direction : (-1 * direction);
  403. }
  404. int ResetMotionModel::JudgeTubeHeightLandmarkDirection()
  405. {
  406. int direction = m_tubeHeightAxisPositiveDirection > 0 ? 1 : -1;
  407. return m_tubeHeightLandmarkDirection > 0 ? (-1 * direction) : direction;
  408. }
  409. float ResetMotionModel::GetTubeRotateResetPosition()
  410. {
  411. return m_tubeRotateResetAngle;
  412. }
  413. int ResetMotionModel::JudgeRotateDirection(float currentAngle, float targetAngle)
  414. {
  415. int direction = m_tubeRotateAxisPositiveDirection > 0 ? 1 : -1;
  416. if (currentAngle > targetAngle)
  417. {
  418. return -1 * direction;
  419. }
  420. if (currentAngle < targetAngle)
  421. {
  422. return 1 * direction;
  423. }
  424. return 0;
  425. }
  426. int ResetMotionModel::JudgeHeightDirection(float currentHeight, float targetHeight)
  427. {
  428. int direction = m_tubeHeightAxisPositiveDirection > 0 ? 1 : -1;
  429. if (currentHeight < targetHeight)
  430. {
  431. return -1 * direction;
  432. }
  433. if (currentHeight > targetHeight)
  434. {
  435. return 1 * direction;
  436. }
  437. return 0;
  438. }
  439. int ResetMotionModel::ComputeTubeAngleZToLandmarkRotateDirection()
  440. {
  441. return -1 * JudgeTubeRotateLandmarkDirection();
  442. }
  443. int ResetMotionModel::ComputeTubeHeightZToLandmarkMoveDirection()
  444. {
  445. return -1 * JudgeTubeHeightLandmarkDirection();
  446. }
  447. BOOL ResetMotionModel::IsTubeAngleAtResetLandmark()
  448. {
  449. BOOL ret = FALSE;
  450. if (m_tubeRotateLandmarkDirection > 0)
  451. {
  452. auto highstatus = m_landmark->ReadTubeAngleLowLandmarkStatus();
  453. ret = (highstatus == 1);
  454. if (ret)
  455. {
  456. m_stageArgs->ActivedTubeAngleLandmark = 1;
  457. }
  458. }
  459. else
  460. {
  461. auto lowstatus = m_landmark->ReadTubeAngleHighLandmarkStatus();
  462. ret = (lowstatus == 1);
  463. if (ret)
  464. {
  465. m_stageArgs->ActivedTubeAngleLandmark = 0;
  466. }
  467. }
  468. return ret;
  469. }
  470. BOOL ResetMotionModel::IsTubeHeightAtResetLandmark()
  471. {
  472. BOOL ret = FALSE;
  473. if (m_tubeHeightLandmarkDirection > 0)
  474. {
  475. auto highstatus = m_landmark->ReadTubeHeightHighLandmarkStatus();
  476. ret = (highstatus == 1);
  477. if (ret)
  478. {
  479. m_stageArgs->ActivedTubeHeightLandmark = 1;
  480. }
  481. }
  482. else
  483. {
  484. auto lowstatus = m_landmark->ReadTubeHeightLowLandmarkStatus();
  485. ret = (lowstatus == 1);
  486. if (ret)
  487. {
  488. m_stageArgs->ActivedTubeHeightLandmark = 0;
  489. }
  490. }
  491. return ret;
  492. }
  493. void ResetMotionModel::SwitchScanningComponents(int nSwitch)
  494. {
  495. }
  496. void ResetMotionModel::SwitchWorkstation(string ws)
  497. {
  498. m_CurWS = ws;
  499. }
  500. void ResetMotionModel::OnStageTubeHorizontalMoveToLandmark()
  501. {
  502. auto tubehight = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HEIGHT);
  503. if (tubehight < m_tubeHeightLowPositionofRest)
  504. {
  505. if (gbusinessLog) gbusinessLog->Warn("[ResetMotionModel][OnStageTubeHorizontalMoveToLandmark]->[tube height is lower {$:f6} than lowest position of tube height config {$:f6}, no moving]", tubehight, m_tubeHeightLowPositionofRest);
  506. return;
  507. }
  508. auto currenthorizontalpos = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HORIZONTAL);
  509. auto targetpos = m_coordinates->GetLandmarkPosition(TOMO_TUBE_HORIZONTAL, LANDMARK_LOW);
  510. int direction = JudgeHorizontalDirection(currenthorizontalpos, targetpos);
  511. auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HORIZONTAL, abs(currenthorizontalpos - targetpos));
  512. MoveTubeHorizontal(direction, steps, m_resetHorizontalPeriod);
  513. }
  514. void ResetMotionModel::OnStageTubeHorizontalFindLandmark()
  515. {
  516. if (IsTubeHorizontalAtResetLandmark())
  517. {
  518. m_stageArgs->IsTubeHorizontalAtResetLandmark = TRUE;
  519. return;
  520. }
  521. int direction = JudgeTubeHorizontalLandmarkDirection();
  522. MoveTubeHorizontal(direction, -1, m_resetHorizontalPeriod);
  523. }
  524. void ResetMotionModel::OnStageTubeHorizontalStopAtLandmark()
  525. {
  526. if (m_tubeLineMotionSwitch)
  527. {
  528. m_tubeLineMotionSwitch->OutputSignal(true);
  529. }
  530. if (m_motorTubeHorizontal)
  531. {
  532. m_motorTubeHorizontal->StopMove();
  533. }
  534. }
  535. void ResetMotionModel::OnStageTubeHorizontalGoOutOfLeftLandmark()
  536. {
  537. int direction = ComputeTubeHorizontalZToLandmarkMoveDirection();
  538. MoveTubeHorizontal(direction, -1, m_resetHorizontalPeriod);
  539. }
  540. void ResetMotionModel::OnStageTubeHorizontalGoOutOfRightLandmark()
  541. {
  542. int direction = ComputeTubeHorizontalZToLandmarkMoveDirection();
  543. MoveTubeHorizontal(direction, -1, m_resetHorizontalPeriod);
  544. }
  545. void ResetMotionModel::OnStageTubeHorizontalAttachZAxis()
  546. {
  547. if (m_encoderTubeHorizontal)
  548. {
  549. m_encoderTubeHorizontal->BindABAxis();
  550. m_encoderTubeHorizontal->BindZAxis();
  551. m_encoderTubeHorizontal->ActiveAutoNotifyWhenZClear(true);
  552. }
  553. }
  554. void ResetMotionModel::OnStageTubeHorizontalDetachZAxis()
  555. {
  556. if (m_encoderTubeHorizontal)
  557. {
  558. m_encoderTubeHorizontal->UnBindZAxis();
  559. m_encoderTubeHorizontal->ActiveAutoNotifyWhenZClear(false);
  560. }
  561. }
  562. void ResetMotionModel::OnStageTubeHorizontalFindZAxis()
  563. {
  564. int direction = ComputeTubeHorizontalZToLandmarkMoveDirection();
  565. MoveTubeHorizontal(direction, -1, m_resetHorizontalPeriod);
  566. }
  567. void ResetMotionModel::OnStageTubeHorizontalStopAtZxis()
  568. {
  569. if (m_motorTubeHorizontal)
  570. {
  571. m_motorTubeHorizontal->StopMove();
  572. }
  573. }
  574. void ResetMotionModel::OnStageTubeHorizontalFinishReset()
  575. {
  576. auto tubehight = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HEIGHT);
  577. if (tubehight < m_tubeHeightLowPositionofRest)
  578. {
  579. if (gbusinessLog) gbusinessLog->Warn("[ResetMotionModel][OnStageTubeHorizontalMoveToLandmark]->[tube height is lower {$:f6} than lowest position of tube height config {$:f6}, no moving]", tubehight, m_tubeHeightLowPositionofRest);
  580. return;
  581. }
  582. auto detectorhorizontal = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HORIZONTAL);
  583. //auto tubehorizontalpos = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HORIZONTAL);
  584. auto tubehorizontalpos = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HORIZONTAL);
  585. auto abshorizontalpos = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HORIZONTAL);
  586. int direction = JudgeHorizontalDirection(abshorizontalpos, detectorhorizontal);
  587. if (gbusinessLog) gbusinessLog->Info("[ResetMotionModel][OnStageTubeHorizontalFinishReset]->[{$:f6} {$:f6} {$:f6}]", abshorizontalpos, detectorhorizontal, tubehorizontalpos);
  588. auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HORIZONTAL, abs(abshorizontalpos - detectorhorizontal));
  589. MoveTubeHorizontal(direction, steps, m_resetHorizontalPeriod);
  590. }
  591. int ResetMotionModel::JudgeTubeHorizontalLandmarkDirection()
  592. {
  593. int direction = m_tubeHorizontalAxisPositiveDirection > 0 ? 1 : -1;
  594. return m_tubeHorizontalLandmarkDirection > 0 ? (-1 * direction) : direction;
  595. }
  596. int ResetMotionModel::JudgeHorizontalDirection(float currentHorizontalPos, float targetHorizontalPos)
  597. {
  598. int direction = m_tubeHorizontalAxisPositiveDirection > 0 ? 1 : -1;
  599. if (currentHorizontalPos < targetHorizontalPos)
  600. {
  601. return -1 * direction;
  602. }
  603. if (currentHorizontalPos > targetHorizontalPos)
  604. {
  605. return 1 * direction;
  606. }
  607. return 0;
  608. }
  609. int ResetMotionModel::ComputeTubeHorizontalZToLandmarkMoveDirection()
  610. {
  611. return -1 * JudgeTubeHorizontalLandmarkDirection();
  612. }
  613. BOOL ResetMotionModel::IsTubeHorizontalAtResetLandmark()
  614. {
  615. BOOL ret = FALSE;
  616. if (m_tubeHorizontalLandmarkDirection > 0)
  617. {
  618. auto rightstatus = m_landmark->ReadTubeHorizontalRightLandmarkStatus();
  619. ret = (rightstatus == 1);
  620. if (ret)
  621. {
  622. m_stageArgs->ActivedTubeHorizontalLandmark = 1;
  623. }
  624. }
  625. else
  626. {
  627. auto leftstatus = m_landmark->ReadTubeHorizontalLeftLandmarkStatus();
  628. ret = (leftstatus == 1);
  629. if (ret)
  630. {
  631. m_stageArgs->ActivedTubeHorizontalLandmark = 0;
  632. }
  633. }
  634. return ret;
  635. }
  636. void ResetMotionModel::MoveTubeHorizontal(int direction, int steps, int pwmperiod)
  637. {
  638. if (m_tubeLineMotionSwitch)
  639. {
  640. m_tubeLineMotionSwitch->OutputSignal(true);
  641. }
  642. if (m_motorTubeHorizontal)
  643. {
  644. m_motorTubeHorizontal->Move(direction, steps, pwmperiod);
  645. }
  646. }
  647. void ResetMotionModel::MoveTubeHeight(int direction, int steps, int pwmperiod)
  648. {
  649. if (m_tubeLineMotionSwitch)
  650. {
  651. m_tubeLineMotionSwitch->OutputSignal(false);
  652. }
  653. if (m_motorTubeHeight)
  654. {
  655. m_motorTubeHeight->Move(direction, steps, pwmperiod);
  656. }
  657. }
  658. void ResetMotionModel::MoveTubeAngle(float despos)
  659. {
  660. auto tubeangle = m_coordinates->GetCurrentPhysical(TOMO_TUBE_ANGLE);
  661. int direction = JudgeRotateDirection(tubeangle, despos);
  662. auto absAngle = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_ANGLE);
  663. if (gbusinessLog) gbusinessLog->Info("[ResetMotionModel][MoveTubeAngle]->[{$:f6} {$:f6} {$:f6}]", absAngle, despos, tubeangle);
  664. auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_ANGLE, abs(tubeangle - despos));
  665. m_motorTubeAngle->Rotate(direction, steps, m_resetRotatePeriod);
  666. }
  667. void ResetMotionModel::OnMotionStageMove2SID()
  668. {
  669. string ws = ConfigurerWS::GetDefaultWS();
  670. if (gmotionLog) gmotionLog->Info("[ResetMotionModel][OnMotionStageMove2SID]->[{$}]", ws.c_str());
  671. if (ws == WS_WALL)
  672. {
  673. float desPos = ConfigurerWS::GetDefaultSIDWall();
  674. float tubeHorizontalPos = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HORIZONTAL);
  675. int direction = JudgeHorizontalDirection(tubeHorizontalPos, desPos);
  676. if (gmotionLog) gmotionLog->Info("[ResetMotionModel][OnMotionStageMove2SID]->[MoveTubeHorizontal Des pos {$:f6}, Cur pos {$:f6}]", desPos, tubeHorizontalPos);
  677. auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HORIZONTAL, abs(tubeHorizontalPos - desPos));
  678. MoveTubeHorizontal(direction, steps, m_resetHorizontalPeriod);
  679. }
  680. else if (ws == WS_TABLE)
  681. {
  682. float desPos = ConfigurerWS::GetDefaultSIDTable();
  683. float tubeHeightPos = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HEIGHT);
  684. int direction = JudgeHeightDirection(tubeHeightPos, desPos);
  685. if (gmotionLog) gmotionLog->Info("[ResetMotionModel][OnMotionStageMove2SID]->[MoveTubeHeight Des pos {$:f6}, Cur pos {$:f6}]", desPos, tubeHeightPos);
  686. auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HEIGHT, abs(tubeHeightPos - desPos));
  687. MoveTubeHeight(direction, steps, m_resetHeightPeriod);
  688. //MoveTubeAngle(-90.0);
  689. }
  690. }