CArmResetMotionModel.cpp 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401
  1. #include "stdafx.h"
  2. #include "CArmResetMotionModel.h"
  3. #include "ResetMotionStageArgs.h"
  4. #include "MotionStages.h"
  5. #include "IMachineryManager.h"
  6. #include "CArmComponentNameDef.h"
  7. #include "ICircularController.h"
  8. #include "ISwingController.h"
  9. #include "ISensorEncoderController.h"
  10. #include "ILandmarkController.h"
  11. #include "IPositionManager.h"
  12. #include "FluoroSwitchController.h"
  13. using namespace DIOS::Dev::Detail::MachineryECOM;
  14. CArmResetMotionModel::CArmResetMotionModel()
  15. :m_StageName(""),
  16. m_stageArgs(new ResetMotionStageArgs()),
  17. m_coordinates(nullptr),
  18. m_circular(nullptr),
  19. m_swing(nullptr),
  20. m_circularencoder(nullptr),
  21. m_swingencoder(nullptr),
  22. m_landmark(nullptr),
  23. m_fluoroSwitch(nullptr),
  24. m_resetSwingPeriod(250),
  25. m_resetCircularPeriod(250),
  26. m_tubeSwingLandmarkDirection(0),
  27. m_tubeCircularLandmarkDirection(0),
  28. m_tubeSwingResetAngle(0.0f),
  29. m_tubeCircularResetAngle(0.0f),
  30. m_tubeCircularAxisPositiveDirection(1),
  31. m_tubeSwingAxisPositiveDirection(1)
  32. {
  33. }
  34. CArmResetMotionModel::~CArmResetMotionModel()
  35. {
  36. if (m_stageArgs)
  37. {
  38. delete m_stageArgs;
  39. m_stageArgs = nullptr;
  40. }
  41. }
  42. void CArmResetMotionModel::ChangeStage(const std::string &stageName)
  43. {
  44. if (gmotionLog) gmotionLog->Info("[CArmResetMotionModel][ChangeStage]->[{$}]", stageName.c_str());
  45. m_StageName = stageName;
  46. OnMotionStage(m_StageName);
  47. }
  48. std::string CArmResetMotionModel::GetStageName()
  49. {
  50. return m_StageName;
  51. }
  52. IMotionStageArgs *CArmResetMotionModel::GetStageArgs()
  53. {
  54. return m_stageArgs;
  55. }
  56. void CArmResetMotionModel::Initialize(IMachineryManager *machineryManager, IPositionManager *coordinates)
  57. {
  58. m_coordinates = coordinates;
  59. m_circular = (ICircularController*)machineryManager->Resove(CONTROLLER_CARM_TUBE_CIRCULAR);
  60. m_swing = (ISwingController*)machineryManager->Resove(CONTROLLER_CARM_TUBE_SWING);
  61. m_circularencoder = (ISensorEncoderController*)machineryManager->Resove(CONTROLLER_CARM_TUBE_CIRCULAR_ENCODER);
  62. m_swingencoder = (ISensorEncoderController*)machineryManager->Resove(CONTROLLER_CARM_TUBE_SWING_ENCODER);
  63. m_landmark = (ILandmarkController *)(machineryManager->Resove(CONTROLLER_LANDMARK));
  64. m_fluoroSwitch = (IOutputController*)(machineryManager->Resove(CONTROLLER_FLUOROSWITCH));
  65. }
  66. void CArmResetMotionModel::LoadMachineryParams(ResDataObject &params)
  67. {
  68. m_resetSwingPeriod = (DWORD)atoi((const char *)params["ResetRotatePeriod"]);
  69. m_resetCircularPeriod = (DWORD)atoi((const char *)params["ResetHeightPeriod"]);
  70. m_tubeSwingLandmarkDirection = (int)atoi((const char *)params["TubeRotateLandmarkDirection"]);
  71. m_tubeCircularLandmarkDirection = (int)atoi((const char *)params["TubeHeightLandmarkDirection"]);
  72. m_tubeSwingResetAngle = (float)atof((const char *)params["TubeRotateResetAngle"]);
  73. m_tubeCircularResetAngle = (float)atof((const char *)params["TubeHeightResetPos"]);
  74. m_tubeCircularAxisPositiveDirection = (int)atoi((const char *)params["TubeHeightAxisPositiveDirection"]);
  75. m_tubeSwingAxisPositiveDirection = (int)atoi((const char *)params["TubeRotateAxisPositiveDirection"]);
  76. }
  77. void CArmResetMotionModel::LoadModelParams(ResDataObject &params)
  78. {
  79. }
  80. void CArmResetMotionModel::SetTechnicalParams(ResDataObject &params)
  81. {
  82. }
  83. void CArmResetMotionModel::OnFeedbackMotionParams(ResDataObject &params)
  84. {
  85. }
  86. BOOL CArmResetMotionModel::GetMotionParams(ResDataObject &params)
  87. {
  88. return TRUE;
  89. }
  90. void CArmResetMotionModel::OnMotionStage(const std::string &stageName)
  91. {
  92. if (stageName == RESET_STAGE_STOP_MOVE)
  93. {
  94. OnMotionStageStopMove();
  95. }
  96. else if (stageName == CARM_RESET_TUBE_SWING_FIND_LANDMARK)
  97. {
  98. OnMotionStageFindSwingLandmark();
  99. }
  100. else if (stageName == CARM_RESET_TUBE_CIRCULAR_FIND_LANDMARK)
  101. {
  102. OnMotionStageFindCircularLandmark();
  103. }
  104. else if (stageName == CARM_RESET_TUBE_SWING_ATTACH_Z_AXIS)
  105. {
  106. OnMotionStageAttachSwingZAxis();
  107. }
  108. else if (stageName == CARM_RESET_TUBE_SWING_DETACH_Z_AXIS)
  109. {
  110. OnMotionStageDetachSwingZAxis();
  111. }
  112. else if (stageName == CARM_RESET_TUBE_CIRCULAR_ATTACH_Z_AXIS)
  113. {
  114. OnMotionStageAttachCircularZAxis();
  115. }
  116. else if (stageName == CARM_RESET_TUBE_CIRCULAR_DETACH_Z_AXIS)
  117. {
  118. OnMotionStageDetachCircularZAxis();
  119. }
  120. else if (stageName == CARM_RESET_TUBE_SWING_FINISHRESET)
  121. {
  122. OnMotionStageSwingFinish();
  123. }
  124. else if (stageName == CARM_RESET_TUBE_CIRCULAR_FINISHRESET)
  125. {
  126. OnMotionStageCircularFinish();
  127. }
  128. else if (stageName == CARM_RESET_TUBE_SWING_STOP_AT_LANDMARK)
  129. {
  130. OnMotionStageSwingStopAtLandmark();
  131. }
  132. else if (stageName == CARM_RESET_TUBE_CIRCULAR_STOP_AT_LANDMARK)
  133. {
  134. OnMotionStageCircularStopAtLandmark();
  135. }
  136. else if (stageName == CARM_RESET_START_EXPOSURE)
  137. {
  138. OnStartExposure();
  139. }
  140. else if (stageName == CARM_RESET_STOP_EXPOSURE)
  141. {
  142. OnStopExposure();
  143. }
  144. }
  145. void CArmResetMotionModel::OnMotionStageStopMove()
  146. {
  147. m_circular->StopRotation();
  148. m_swing->StopSwing();
  149. }
  150. void CArmResetMotionModel::OnMotionStageFindSwingLandmark()
  151. {
  152. //if (IsTubeSwingAtResetLandmark())
  153. //{
  154. // m_stageArgs->IsTubeAngleAtResetLandmark = TRUE;
  155. // return;
  156. //}
  157. int direction = JudgeTubeSwingLandmarkDirection();
  158. m_swing->Swing(direction, -1, m_resetSwingPeriod);
  159. }
  160. BOOL CArmResetMotionModel::IsTubeSwingAtResetLandmark()
  161. {
  162. BOOL ret = FALSE;
  163. if (m_tubeSwingLandmarkDirection > 0)
  164. {
  165. auto highstatus = m_landmark->ReadTubeAngleHighLandmarkStatus();
  166. ret = (highstatus == 1);
  167. if (ret)
  168. {
  169. m_stageArgs->ActivedTubeAngleLandmark = 1;
  170. }
  171. }
  172. else
  173. {
  174. auto lowstatus = m_landmark->ReadTubeAngleLowLandmarkStatus();
  175. ret = (lowstatus == 1);
  176. if (ret)
  177. {
  178. m_stageArgs->ActivedTubeAngleLandmark = 0;
  179. }
  180. }
  181. return ret;
  182. }
  183. int CArmResetMotionModel::JudgeTubeSwingLandmarkDirection()
  184. {
  185. int direction = m_tubeSwingAxisPositiveDirection > 0 ? 1 : -1;
  186. return m_tubeSwingLandmarkDirection > 0 ? direction : (-1 * direction);
  187. }
  188. void CArmResetMotionModel::OnMotionStageFindCircularLandmark()
  189. {
  190. //if (IsTubeCircularAtResetLandmark())
  191. //{
  192. // if (gmotionLog) gmotionLog->Info("[CArmResetMotionModel][OnMotionStageFindCircularLandmark]->[Is At Landmark]");
  193. // m_stageArgs->IsTubeHeightAtResetLandmark = TRUE;
  194. // return;
  195. //}
  196. int direction = JudgeTubeCircularLandmarkDirection();
  197. if (gmotionLog) gmotionLog->Info("[CArmResetMotionModel][OnMotionStageFindCircularLandmark]->[dir {$:d}]", direction);
  198. m_circular->Rotate(direction, -1, m_resetCircularPeriod);
  199. }
  200. BOOL CArmResetMotionModel::IsTubeCircularAtResetLandmark()
  201. {
  202. BOOL ret = FALSE;
  203. if (m_tubeCircularLandmarkDirection > 0)
  204. {
  205. auto highstatus = m_landmark->ReadTubeHeightHighLandmarkStatus();
  206. ret = (highstatus == 0/*1*/);
  207. if (ret)
  208. {
  209. m_stageArgs->ActivedTubeHeightLandmark = 1;
  210. }
  211. }
  212. else
  213. {
  214. auto lowstatus = m_landmark->ReadTubeHeightLowLandmarkStatus();
  215. ret = (lowstatus == 0/*1*/);
  216. if (ret)
  217. {
  218. m_stageArgs->ActivedTubeHeightLandmark = 0;
  219. }
  220. }
  221. return ret;
  222. }
  223. int CArmResetMotionModel::JudgeTubeCircularLandmarkDirection()
  224. {
  225. if (gmotionLog) gmotionLog->Info("[CArmResetMotionModel][JudgeTubeCircularLandmarkDirection]->[Positive:{$:d} Landmark:{$:d}]",
  226. m_tubeCircularAxisPositiveDirection, m_tubeCircularLandmarkDirection);
  227. int direction = m_tubeCircularAxisPositiveDirection > 0 ? 1 : -1;
  228. return m_tubeCircularLandmarkDirection > 0 ? direction : (-1 * direction);
  229. }
  230. void CArmResetMotionModel::OnMotionStageAttachSwingZAxis()
  231. {
  232. m_swingencoder->BindABAxis();
  233. m_swingencoder->BindZAxis();
  234. m_swingencoder->ActiveAutoNotifyWhenZClear(true);
  235. }
  236. void CArmResetMotionModel::OnMotionStageDetachSwingZAxis()
  237. {
  238. m_swingencoder->UnBindZAxis();
  239. m_swingencoder->ActiveAutoNotifyWhenZClear(false);
  240. }
  241. void CArmResetMotionModel::OnMotionStageAttachCircularZAxis()
  242. {
  243. m_circularencoder->BindABAxis();
  244. m_circularencoder->BindZAxis();
  245. m_circularencoder->ActiveAutoNotifyWhenZClear(true);
  246. }
  247. void CArmResetMotionModel::OnMotionStageDetachCircularZAxis()
  248. {
  249. m_circularencoder->UnBindZAxis();
  250. m_circularencoder->ActiveAutoNotifyWhenZClear(false);
  251. }
  252. void CArmResetMotionModel::OnMotionStageSwingFinish()
  253. {
  254. auto resetAngle = GetTubeSwingResetPosition();
  255. auto tubeangle = m_coordinates->GetCurrentPhysical(CARM_TUBE_SWING);
  256. int direction = -1 * JudgeTubeSwingLandmarkDirection();
  257. if (gmotionLog) gmotionLog->Info("[CArmResetMotionModel][OnStageTubeSwingFinishReset]->[ResetPos:{$:f6} Current:{$:f6}]",resetAngle, tubeangle);
  258. auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_CARM_TUBE_SWING, abs(tubeangle - resetAngle));
  259. m_swing->Swing(direction, steps, m_resetSwingPeriod);
  260. }
  261. float CArmResetMotionModel::GetTubeSwingResetPosition()
  262. {
  263. return m_tubeSwingResetAngle;
  264. }
  265. int CArmResetMotionModel::JudgeSwingDirection(float current, float target)
  266. {
  267. int direction = m_tubeSwingAxisPositiveDirection > 0 ? 1 : -1;
  268. if (current > target)
  269. {
  270. return -1 * direction;
  271. }
  272. if (current < target)
  273. {
  274. return 1 * direction;
  275. }
  276. return 0;
  277. }
  278. void CArmResetMotionModel::OnMotionStageCircularFinish()
  279. {
  280. auto resetAngle = GetTubeCircularResetPosition();
  281. auto tubeangle = m_coordinates->GetCurrentPhysical(CARM_TUBE_CIRCULAR);
  282. int direction = -1 * JudgeTubeCircularLandmarkDirection();
  283. if (gmotionLog) gmotionLog->Info("[CArmResetMotionModel][OnStageTubeCircularFinishReset]->[ResetPos:{$:f6} Current:{$:f6}]", resetAngle, tubeangle);
  284. auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_CARM_TUBE_CIRCULAR, abs(tubeangle - resetAngle));
  285. m_circular->Rotate(direction, steps, m_resetCircularPeriod);
  286. }
  287. float CArmResetMotionModel::GetTubeCircularResetPosition()
  288. {
  289. return m_tubeCircularResetAngle;
  290. }
  291. int CArmResetMotionModel::JudgeCircularDirection(float current, float target)
  292. {
  293. int direction = m_tubeCircularAxisPositiveDirection > 0 ? 1 : -1;
  294. if (current > target)
  295. {
  296. return -1 * direction;
  297. }
  298. if (current < target)
  299. {
  300. return 1 * direction;
  301. }
  302. return 0;
  303. }
  304. void CArmResetMotionModel::OnMotionStageSwingStopAtLandmark()
  305. {
  306. m_swing->StopSwing();
  307. }
  308. void CArmResetMotionModel::OnMotionStageCircularStopAtLandmark()
  309. {
  310. m_circular->StopRotation();
  311. }
  312. void CArmResetMotionModel::SwitchScanningComponents(int nSwitch)
  313. {
  314. }
  315. void CArmResetMotionModel::OnStartExposure()
  316. {
  317. if (m_fluoroSwitch)
  318. {
  319. if (gbusinessLog) gbusinessLog->Info("[CArmResetMotionModel][OnStartExposure]->[Enable fluoro switch signal]");
  320. m_fluoroSwitch->OutputSignal(true);
  321. }
  322. }
  323. void CArmResetMotionModel::OnStopExposure()
  324. {
  325. if (m_fluoroSwitch)
  326. {
  327. if (gbusinessLog) gbusinessLog->Info("[CArmResetMotionModel][OnStopExposure]->[Disable fluoro switch signal]");
  328. m_fluoroSwitch->OutputSignal(false);
  329. }
  330. }
  331. void CArmResetMotionModel::SwitchWorkstation(string ws)
  332. {
  333. m_CurWS = ws;
  334. }