CBCTMotionModel.cpp 13 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450
  1. #include "stdafx.h"
  2. #include "CBCTMotionModel.h"
  3. #include "CBCTMotionStageArgs.h"
  4. #include "ICircularController.h"
  5. #include "ISwingController.h"
  6. #include "ISensorEncoderController.h"
  7. #include "IMachineryManager.h"
  8. #include "CArmComponentNameDef.h"
  9. #include "MotionStages.h"
  10. #include "IPositionManager.h"
  11. #include "IExposureController.h"
  12. #include "FeedbackDefine.h"
  13. #include "ConfigurerMotion.h"
  14. #include "FluoroSwitchController.h"
  15. using namespace DIOS::Dev::Detail::MachineryECOM;
  16. CBCTMotionModel::CBCTMotionModel()
  17. :m_stageArgs(new CBCTMotionStageArgs()),
  18. m_coordinates(nullptr),
  19. m_machineryManager(nullptr),
  20. m_circular(nullptr),
  21. m_swing(nullptr),
  22. m_circularencoder(nullptr),
  23. m_swingencoder(nullptr),
  24. m_exposure(nullptr),
  25. m_fluoroSwitch(nullptr),
  26. m_bUseCARMRotationScanning(false)
  27. {
  28. }
  29. CBCTMotionModel::~CBCTMotionModel()
  30. {
  31. }
  32. void CBCTMotionModel::ChangeStage(const std::string &stageName)
  33. {
  34. m_StageName = stageName;
  35. OnMotionStage(stageName);
  36. }
  37. std::string CBCTMotionModel::GetStageName()
  38. {
  39. return m_StageName;
  40. }
  41. IMotionStageArgs *CBCTMotionModel::GetStageArgs()
  42. {
  43. return m_stageArgs;
  44. }
  45. void CBCTMotionModel::Initialize(IMachineryManager *machineryManager, IPositionManager *coordinates)
  46. {
  47. m_coordinates = coordinates;
  48. m_machineryManager = machineryManager;
  49. m_circular = (ICircularController*)machineryManager->Resove(CONTROLLER_CARM_TUBE_CIRCULAR);
  50. m_swing = (ISwingController*)machineryManager->Resove(CONTROLLER_CARM_TUBE_SWING);
  51. m_circularencoder = (ISensorEncoderController*)machineryManager->Resove(CONTROLLER_CARM_TUBE_CIRCULAR_ENCODER);
  52. m_swingencoder = (ISensorEncoderController*)machineryManager->Resove(CONTROLLER_CARM_TUBE_SWING_ENCODER);
  53. m_exposure = (IExposureController *)(machineryManager->Resove(CONTROLLER_EXPOSURE));
  54. m_fluoroSwitch = (IOutputController *)(machineryManager->Resove(CONTROLLER_FLUOROSWITCH));
  55. m_bUseCARMRotationScanning = ConfigurerMotion::GetUseSwingScanning();
  56. }
  57. void CBCTMotionModel::LoadMachineryParams(ResDataObject &params)
  58. {
  59. m_MachineryParams = params;
  60. }
  61. void CBCTMotionModel::LoadModelParams(ResDataObject &params)
  62. {
  63. m_ModelParams = params;
  64. }
  65. void CBCTMotionModel::SetTechnicalParams(ResDataObject &params)
  66. {
  67. m_TechnicalParams = params;
  68. }
  69. void CBCTMotionModel::OnFeedbackMotionParams(ResDataObject &params)
  70. {
  71. std::string key((const char *)params["key"]);
  72. if (key == CARM_MODLE_FEEDBACK_KEY_CIRCULARENCODER)
  73. {
  74. m_FeedbackTubeCircularEncoders.push_back((int)params[key.c_str()]);
  75. }
  76. }
  77. BOOL CBCTMotionModel::GetMotionParams(ResDataObject &params)
  78. {
  79. std::string key = (const char *)params["key"];
  80. if (key == "CircularResult")
  81. {
  82. params.clear();
  83. int count = 0;
  84. for (auto item : m_FeedbackTubeCircularEncoders)
  85. {
  86. float height = m_coordinates->ConvertSensorValue(m_circularencoder->Name(), item);
  87. char buffer[10];
  88. memset(buffer, 0, 10);
  89. sprintf_s(buffer, "%d", count);
  90. params.add(buffer, height * 100);
  91. ++count;
  92. }
  93. }
  94. return TRUE;
  95. }
  96. void CBCTMotionModel::OnMotionStage(const std::string &stageName)
  97. {
  98. if (stageName == CBCT_STAGE_CALCULATE_PARAMS)
  99. {
  100. MotionStageCalculateParams();
  101. }
  102. else if (stageName == CBCT_STAGE_SET_CBCT_SLICE)
  103. {
  104. MotionStageSetCBCTMotionSlice();
  105. }
  106. else if (stageName == CBCT_STAGE_MOVETO_END_POS)
  107. {
  108. MotionStageMovetoEndPos();
  109. }
  110. else if (stageName == CBCT_STAGE_CLEAR_PARAMS)
  111. {
  112. MotionStageClearParams();
  113. }
  114. else if (stageName == CBCT_STAGE_MOVETO_START_POS)
  115. {
  116. MotionStageMovetoStartPos();
  117. }
  118. else if (stageName == CBCT_STAGE_MOTION_ERROR_STOP)
  119. {
  120. MotionStageErrorStop();
  121. }
  122. else if (stageName == CBCT_STAGE_MOTION_ERROR_RECOVER)
  123. {
  124. MotionStageErrorRecover();
  125. }
  126. else if (stageName == CBCT_STAGE_MOTION_CHANGE_PFS)
  127. {
  128. }
  129. else if (stageName == CBCT_STAGE_START_EXPOSURE)
  130. {
  131. OnStartExposure();
  132. }
  133. else if (stageName == CBCT_STAGE_STOP_EXPOSURE)
  134. {
  135. OnStopExposure();
  136. }
  137. }
  138. void CBCTMotionModel::MotionStageClearParams()
  139. {
  140. m_circularencoder->ClearTrigger();
  141. m_swingencoder->ClearTrigger();
  142. }
  143. void CBCTMotionModel::MotionStageCalculateParams()
  144. {
  145. float acc = strtof((const char*)m_ModelParams["CBCTAcceleratingDistance"], nullptr);
  146. float brk = strtof((const char*)m_ModelParams["CBCTDeceleratingDistance"], nullptr);
  147. float startpos = strtof((const char *)m_TechnicalParams["CBCTStartPos"], nullptr);
  148. float speed = strtof((const char *)m_TechnicalParams["CBCTMotionSpeed"], nullptr);
  149. int nproject = (int)m_TechnicalParams["CBCTProjectionNumber"];
  150. float scnAngle = strtof((const char *)m_TechnicalParams["CBCTScanAngle"], nullptr);
  151. int direction = (int)m_TechnicalParams["CBCTProjectionDirection"];
  152. m_startPos = startpos - (direction) * acc;
  153. m_endPos = m_startPos + (direction)*(scnAngle + brk);
  154. if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][MotionStageCalculateParams]->[S:{$:f3} E:{$:f3}]", m_startPos, m_endPos);
  155. m_exposureTriggerSlice.clear();
  156. for (int i = 0; i < nproject; ++i)
  157. {
  158. float anglePerProject = scnAngle / (nproject * 1.0f);
  159. float triggerAngle = startpos - i * anglePerProject;
  160. m_exposureTriggerSlice.push_back(triggerAngle);
  161. }
  162. }
  163. void CBCTMotionModel::MotionStageSetCBCTMotionSlice()
  164. {
  165. if (!m_bUseCARMRotationScanning)
  166. {
  167. //设置编码器触发同步使能
  168. auto exposureTriggerID = m_exposure->GetInterfaceID(ID_COMMUNICATE_INTERFACE);
  169. m_circularencoder->ActiveExposureTrigger(exposureTriggerID);
  170. //依次设置曝光点位关联同步信号
  171. int nSize = m_exposureTriggerSlice.size();
  172. for (int i = 0; i < nSize; ++i)
  173. {
  174. if (gbusinessLog) gbusinessLog->Info("[CBCTMotionModel][SetExposureTrigger]->[Circular][Total:{$:d}, Cur:{$:d}][{$:f3}]", nSize, i, m_exposureTriggerSlice[i]);
  175. DWORD encoder = m_coordinates->ConvertPhysicsValue(CONTROLLER_CARM_TUBE_CIRCULAR_ENCODER, m_exposureTriggerSlice[i]);
  176. m_circularencoder->SetExposureTrigger(encoder);
  177. }
  178. //设置曝光信号触发编码器值回传
  179. auto sensortubeHeightID = m_circularencoder->GetInterfaceID(ID_COMMUNICATE_INTERFACE);
  180. m_exposure->ActiveExposureTubeHeightPositionAutoNotify(sensortubeHeightID);
  181. }
  182. else
  183. {
  184. auto exposureTriggerID = m_exposure->GetInterfaceID(ID_COMMUNICATE_INTERFACE);
  185. m_swingencoder->ActiveExposureTrigger(exposureTriggerID);
  186. int nSize = m_exposureTriggerSlice.size();
  187. for (int i = 0; i < nSize; ++i)
  188. {
  189. if (gbusinessLog) gbusinessLog->Info("[CBCTMotionModel][SetExposureTrigger]->[Swing][Total:{$:d}, Cur:{$:d}][{$:f3}]", nSize, i, m_exposureTriggerSlice[i]);
  190. DWORD encoder = m_coordinates->ConvertPhysicsValue(CONTROLLER_CARM_TUBE_SWING_ENCODER, m_exposureTriggerSlice[i]);
  191. m_swingencoder->SetExposureTrigger(encoder);
  192. }
  193. auto sensortubeHeightID = m_swingencoder->GetInterfaceID(ID_COMMUNICATE_INTERFACE);
  194. m_exposure->ActiveExposureTubeAnglePositionAutoNotify(sensortubeHeightID);
  195. }
  196. }
  197. void CBCTMotionModel::MotionStageMovetoEndPos()
  198. {
  199. if (!m_bUseCARMRotationScanning)
  200. {
  201. if (!m_coordinates->IsSystemReady(CONTROLLER_CARM_TUBE_CIRCULAR))
  202. {
  203. if (gmotionLog) gmotionLog->Warn("[CBCTMotionModel][MotionStageMovetoEndPos]->[CONTROLLER_CARM_TUBE_CIRCULAR is not ready status, need to reset first]");
  204. return;
  205. }
  206. float currentPosition = m_coordinates->GetCurrentPhysical(CARM_TUBE_CIRCULAR);
  207. int direction = JudgeDirectionInTubeHeightAxis(currentPosition, m_endPos);
  208. if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][MotionStageMovetoEndPos]->[Current Position:{$:f3} Target Position:{$:f3} Direction:{$:d}]",
  209. currentPosition, m_endPos, direction);
  210. MoveTubeCircular(direction, abs(currentPosition - m_endPos));
  211. }
  212. else
  213. {
  214. if (!m_coordinates->IsSystemReady(CONTROLLER_CARM_TUBE_SWING))
  215. {
  216. if (gmotionLog) gmotionLog->Warn("[CBCTMotionModel][MotionStageMovetoEndPos]->[CONTROLLER_CARM_TUBE_SWING is not ready status, need to reset first]");
  217. return;
  218. }
  219. float currentPosition = m_coordinates->GetCurrentPhysical(CARM_TUBE_SWING);
  220. int direction = JudgeDirectionInTubeSwingAxis(currentPosition, m_endPos);
  221. if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][MotionStageMovetoEndPos]->[Current Position:{$:f3} Target Position:{$:f3} Direction:{$:d}]",
  222. currentPosition, m_endPos, direction);
  223. RotateTubeSwing(direction, abs(currentPosition - m_endPos));
  224. }
  225. }
  226. void CBCTMotionModel::MotionStageMovetoStartPos()
  227. {
  228. if (!m_bUseCARMRotationScanning)
  229. {
  230. TubeCircularToStartPosition();
  231. }
  232. else
  233. {
  234. TubeSwingToStartPostion();
  235. }
  236. }
  237. void CBCTMotionModel::MotionStageErrorStop()
  238. {
  239. if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][MotionStageErrorStop]->[Enter]");
  240. m_circular->StopRotation();
  241. m_swing->StopSwing();
  242. if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][MotionStageErrorStop]->[Leave]");
  243. }
  244. void CBCTMotionModel::MotionStageErrorRecover()
  245. {
  246. }
  247. void CBCTMotionModel::TubeSwingToStartPostion()
  248. {
  249. if (!m_coordinates->IsSystemReady(CONTROLLER_CARM_TUBE_SWING))
  250. {
  251. if (gmotionLog) gmotionLog->Warn("[CBCTMotionModel][TubeSwingToStartPostion]->[CONTROLLER_CARM_TUBE_SWING is not ready status, need to reset first]");
  252. return;
  253. }
  254. float tubeAngle = m_coordinates->GetCurrentPhysical(CARM_TUBE_SWING);
  255. //auto swingStartPosition = 0.0f;
  256. auto swingStartPosition = m_startPos;
  257. int direction = JudgeDirectionInTubeSwingAxis(tubeAngle, swingStartPosition);
  258. float speed = 0.0f;
  259. if (m_MachineryParams.GetFirstOf("MotionSpeed") > 0)
  260. {
  261. speed = (float)atof((const char *)m_MachineryParams["MotionSpeed"]);
  262. }
  263. if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][TubeSwingToStartPostion]->[S:{$:f3} T:{$:f3} D:{$:d} SP:{$:f3}]",
  264. tubeAngle, m_startPos, direction, speed);
  265. RotateTubeSwing(direction, abs(swingStartPosition - tubeAngle), speed);
  266. }
  267. void CBCTMotionModel::TubeCircularToStartPosition()
  268. {
  269. if (!m_coordinates->IsSystemReady(CONTROLLER_CARM_TUBE_CIRCULAR))
  270. {
  271. if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][TubeCircularToStartPosition]->[CONTROLLER_CARM_TUBE_CIRCULAR is not ready status, need to reset first]");
  272. return;
  273. }
  274. float tubeHeight = m_coordinates->GetCurrentPhysical(CARM_TUBE_CIRCULAR);
  275. int direction = JudgeDirectionInTubeHeightAxis(tubeHeight, m_startPos);
  276. float speed = 0.0f;
  277. if (m_MachineryParams.GetFirstOf("MotionSpeed") > 0)
  278. {
  279. speed = (float)atof((const char *)m_MachineryParams["MotionSpeed"]);
  280. }
  281. if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][TubeCircularToStartPosition]->[S:{$:f3} T:{$:f3} D:{$:d} SP:{$:f3}]", tubeHeight, m_startPos, direction, speed);
  282. MoveTubeCircular(direction, abs(m_startPos - tubeHeight), speed);
  283. }
  284. int CBCTMotionModel::JudgeDirectionInTubeSwingAxis(float current, float target)
  285. {
  286. auto positive = (int)atoi((const char *)m_MachineryParams["TubeRotateAxisPositiveDirection"]);
  287. auto direction = positive > 0 ? 1 : -1;
  288. if (current > target)
  289. {
  290. return -1 * direction;
  291. }
  292. if (current < target)
  293. {
  294. return 1 * direction;
  295. }
  296. return 0;
  297. }
  298. void CBCTMotionModel::RotateTubeSwing(int direction, float offset, float spd)
  299. {
  300. float speed = strtof((const char *)m_TechnicalParams["CBCTMotionSpeed"], nullptr);
  301. if (spd > 1e-3)
  302. {
  303. speed = spd;
  304. }
  305. auto period = m_coordinates->ConvertMotorSpeed(CONTROLLER_CARM_TUBE_SWING, speed);
  306. auto step = m_coordinates->ConvertMotorStepValue(CONTROLLER_CARM_TUBE_SWING, offset);
  307. if (period < 50 || (period - 50) < 30)
  308. {
  309. period = 50;
  310. }
  311. if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][RotateTubeSwing]->[Step:{$:d} Period:{$:d} Direction:{$:d}]", step, period, direction);
  312. m_swing->Swing(direction, step, period);
  313. }
  314. int CBCTMotionModel::JudgeDirectionInTubeHeightAxis(float current, float target)
  315. {
  316. auto positive = (int)atoi((const char *)m_MachineryParams["TubeHeightAxisPositiveDirection"]);
  317. auto direction = positive > 0 ? 1 : -1;
  318. if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][JudgeDirectionInTubeHeightAxis]->[C:{$:f3} T:{$:f3} D:{$:d}]", current, target, direction);
  319. if (current > target)
  320. {
  321. return -1 * direction;
  322. }
  323. if (current < target)
  324. {
  325. return 1 * direction;
  326. }
  327. return 0;
  328. }
  329. void CBCTMotionModel::MoveTubeCircular(int direction, float offset, float spd)
  330. {
  331. float speed = strtof((const char *)m_TechnicalParams["CBCTMotionSpeed"], nullptr);
  332. if (spd > 1e-3)
  333. {
  334. speed = spd;
  335. }
  336. auto step = m_coordinates->ConvertMotorStepValue(CONTROLLER_CARM_TUBE_CIRCULAR, offset);
  337. auto period = m_coordinates->ConvertMotorSpeed(CONTROLLER_CARM_TUBE_CIRCULAR, speed);
  338. if (period < 50 || (period - 50) < 30)
  339. {
  340. period = 50;
  341. }
  342. m_circular->Rotate(direction, step, period);
  343. }
  344. void CBCTMotionModel::SwitchScanningComponents(int nSwitch)
  345. {
  346. //false:滑轨滑动扫描 true:轴向转动扫描
  347. m_bUseCARMRotationScanning = nSwitch;
  348. if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][SwitchScanningComponents]->[Using {$} for scanning]", m_bUseCARMRotationScanning ? "swing" : "circular");
  349. }
  350. void CBCTMotionModel::OnStartExposure()
  351. {
  352. if (m_fluoroSwitch)
  353. {
  354. if (gbusinessLog) gbusinessLog->Info("[CBCTMotionModel][OnStartExposure]->[Enable fluoro switch signal]");
  355. m_fluoroSwitch->OutputSignal(true);
  356. }
  357. }
  358. void CBCTMotionModel::OnStopExposure()
  359. {
  360. if (m_fluoroSwitch)
  361. {
  362. if (gbusinessLog) gbusinessLog->Info("[CBCTMotionModel][OnStopExposure]->[Disable fluoro switch signal]");
  363. m_fluoroSwitch->OutputSignal(false);
  364. }
  365. }
  366. void CBCTMotionModel::SwitchWorkstation(string ws)
  367. {
  368. m_CurWS = ws;
  369. }