CalibrationModel.cpp 15 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556
  1. #include "stdafx.h"
  2. #include "CalibrationModel.h"
  3. #include "MotionStages.h"
  4. #include "CalibrationStageArgs.h"
  5. #include "IMachineryManager.h"
  6. #include "ITubeAngleController.h"
  7. #include "ITubeHeightController.h"
  8. #include "ISensorADController.h"
  9. #include "ISensorEncoderController.h"
  10. #include "IPositionManager.h"
  11. #include "ConfigurerMotion.h"
  12. #include "TubeLineMotionSwitchController.h"
  13. using namespace DIOS::Dev::Detail::MachineryECOM;
  14. CalibrationModel::CalibrationModel()
  15. :m_StageName(""),
  16. m_stageArgs(new CalibrationStageArgs()),
  17. m_coordinates(nullptr),
  18. m_tubeAngle(nullptr),
  19. m_tubeHeight(nullptr),
  20. m_tubeHeightAD(nullptr),
  21. m_tubeAngleAD(nullptr),
  22. m_detectorAD(nullptr),
  23. m_tubeHeightEncoder(nullptr),
  24. m_tubeAngleEncoder(nullptr),
  25. m_detectorHeightEncoder(nullptr),
  26. m_motorTubeHorizontal(nullptr),
  27. m_encoderTubeHorizontal(nullptr),
  28. m_tubeLineMotionSwitch(nullptr),
  29. m_tubeHeightMotionPeriod(150),
  30. m_tubeAngleMotionPeriod(150),
  31. m_tubeRotateLandmarkDirection(1),
  32. m_tubeHeightLandmarkDirection(1),
  33. m_tubeHeightAxisPositiveDirection(1),
  34. m_tubeRotateAxisPositiveDirection(1),
  35. m_autoCalADPhysicalEachStep(0.0f)
  36. {
  37. }
  38. CalibrationModel::~CalibrationModel()
  39. {
  40. if (m_stageArgs)
  41. {
  42. delete m_stageArgs;
  43. m_stageArgs = nullptr;
  44. }
  45. }
  46. void CalibrationModel::ChangeStage(const std::string &stageName)
  47. {
  48. m_StageName = stageName;
  49. OnMotionStage(stageName);
  50. }
  51. std::string CalibrationModel::GetStageName()
  52. {
  53. return m_StageName;
  54. }
  55. IMotionStageArgs *CalibrationModel::GetStageArgs()
  56. {
  57. return m_stageArgs;
  58. }
  59. void CalibrationModel::Initialize(IMachineryManager *machineryManager, IPositionManager *coordinates)
  60. {
  61. m_coordinates = coordinates;
  62. m_tubeAngle = (ITubeAngleController *)(machineryManager->Resove(CONTROLLER_TUBE_ANGLE));
  63. m_tubeHeight = (ITubeHeightController *)(machineryManager->Resove(CONTROLLER_TUBE_HEIGHT));
  64. m_detectorAD = (ISensorADController *)(machineryManager->Resove(CONTROLLER_DETECTOR_HEIGHT_AD));
  65. m_tubeHeightAD = (ISensorADController *)(machineryManager->Resove(CONTROLLER_TUBE_HEIGHT_AD));
  66. m_tubeAngleAD = (ISensorADController *)(machineryManager->Resove(CONTROLLER_TUBE_ANGLE_AD));
  67. m_tubeHeightEncoder = (ISensorEncoderController *)(machineryManager->Resove(CONTROLLER_TUBE_HEIGHT_ENCODER));
  68. m_tubeAngleEncoder = (ISensorEncoderController *)(machineryManager->Resove(CONTROLLER_TUBE_ANGLE_ENCODER));
  69. m_detectorHeightEncoder = (ISensorEncoderController *)(machineryManager->Resove(CONTROLLER_DETECTOR_HEIGHT_ENCODER));
  70. m_motorTubeHorizontal = (ITubeHeightController*)(machineryManager->Resove(CONTROLLER_TUBE_HORIZONTAL));
  71. m_encoderTubeHorizontal = (ISensorEncoderController*)(machineryManager->Resove(CONTROLLER_TUBE_HORIZONTAL_ENCODER));
  72. m_adTubeHorizontal = (ISensorADController*)(machineryManager->Resove(CONTROLLER_TUBE_HORIZONTAL_AD));
  73. m_tubeLineMotionSwitch = (IOutputController*)(machineryManager->Resove(CONTROLLER_TUBE_LINE_MOTION_SWTICH));
  74. m_detectorHorizontalAD = (ISensorADController*)(machineryManager->Resove(CONTROLLER_DETECTOR_HORIZONTAL_AD));
  75. m_detectorHorizontalEncoder = (ISensorEncoderController*)(machineryManager->Resove(CONTROLLER_DETECTOR_HORIZONTAL_ENCODER));
  76. assert(m_tubeAngle);
  77. assert(m_tubeHeight);
  78. assert(m_detectorAD);
  79. assert(m_tubeHeightAD);
  80. assert(m_tubeAngleAD);
  81. assert(m_tubeHeightEncoder);
  82. assert(m_tubeAngleEncoder);
  83. assert(m_detectorHeightEncoder);
  84. }
  85. void CalibrationModel::LoadMachineryParams(ResDataObject &params)
  86. {
  87. m_tubeAngleMotionPeriod = (DWORD)ConfigurerMotion::GetTubeRotateResetPeriod();
  88. m_tubeRotateLandmarkDirection = ConfigurerMotion::GetTubeRotateLandmarkDirection();
  89. m_tubeRotateAxisPositiveDirection = ConfigurerMotion::GetTubeRotateAxisPositiveDirection() > 0 ? 1 : -1;
  90. m_tubeHeightMotionPeriod = (DWORD)ConfigurerMotion::GetTubeHeightResetPeriod();
  91. m_tubeHeightLandmarkDirection = ConfigurerMotion::GetTubeHeightLandmarkDirection();
  92. m_tubeHeightAxisPositiveDirection = ConfigurerMotion::GetTubeHeightAxisPositiveDirection() > 0 ? 1 : -1;
  93. m_tubeHorizontalMotionPeriod = (DWORD)ConfigurerMotion::GetTubeHorizontalResetPeriod();
  94. m_tubeHorizontalLandmarkDirection = ConfigurerMotion::GetTubeHorizontalLandmarkDirection();
  95. m_tubeHorizontalAxisPositiveDirection = ConfigurerMotion::GetTubeHorizontalAxisPositiveDirection() > 0 ? 1 : -1;
  96. }
  97. void CalibrationModel::LoadModelParams(ResDataObject &params)
  98. {
  99. }
  100. void CalibrationModel::SetTechnicalParams(ResDataObject &params)
  101. {
  102. }
  103. void CalibrationModel::OnFeedbackMotionParams(ResDataObject &params)
  104. {
  105. }
  106. BOOL CalibrationModel::GetMotionParams(ResDataObject &params)
  107. {
  108. return TRUE;
  109. }
  110. void CalibrationModel::OnMotionStage(const std::string &name)
  111. {
  112. if (CALIBRATION_MOVE_TUBE_HEIGHT == name)
  113. {
  114. OnStageMoveTubeHeight();
  115. }
  116. else if (name == CALIBRATION_ROTATE_TUBE_ANGLE)
  117. {
  118. OnStageRotateTubeAngle();
  119. }
  120. else if (name == CALIBRATION_GET_TUBE_ANGLE_AD)
  121. {
  122. OnStageGetTubeAngleAD();
  123. }
  124. else if (name == CALIBRATION_GET_TUBE_HEIGHT_AD)
  125. {
  126. OnStageGetTubeHeightAD();
  127. }
  128. else if (name == CALIBRATION_GET_DETECTOR_HEIGHT_AD)
  129. {
  130. OnStageGetDetectorHeightAD();
  131. }
  132. else if (name == CALIBRATION_GET_TUBE_ANGLE_ENCODER)
  133. {
  134. OnStageGetTubeAngleEncoder();
  135. }
  136. else if (name == CALIBRATION_GET_TUBE_HEIGHT_ENCODER)
  137. {
  138. OnStageGetTubeHeightEncoder();
  139. }
  140. else if (name == CALIBRATION_GET_DETECTOR_HEIGHT_ENCODER)
  141. {
  142. OnStageGetDetectorHeightEncoder();
  143. }
  144. else if (name == CALIBRATION_ACTIVE_TUBE_ANGLE_ZAXIS_CLEAR)
  145. {
  146. OnStageActiveTubeAngleZAxisClear();
  147. }
  148. else if (name == CALIBRATION_CANCEL_TUBE_ANGLE_ZAXIS_CLEAR)
  149. {
  150. OnStageCancelTubeAngleZAxisClear();
  151. }
  152. else if (name == CALIBRATION_ACTIVE_TUBE_HEIGHT_ZAXIS_CLEAR)
  153. {
  154. OnStageActiveTubeHeightZAxisClear();
  155. }
  156. else if (name == CALIBRATION_CANCEL_TUBE_HEIGHT_ZAXIS_CLEAR)
  157. {
  158. OnStageCancelTubeHeightZAxisClear();
  159. }
  160. else if (name == CALIBRATION_ROTATE_TUBE_ANGLE_TO_ZAXIS)
  161. {
  162. OnStageRotateTubeAngleToZAxis();
  163. }
  164. else if (name == CALIBRATION_MVOE_TUBE_HEIGHT_TO_ZAXIS)
  165. {
  166. OnStageMoveTubeHeightToZAxis();
  167. }
  168. else if (name == CALIBRATION_STOP_TUBE_ANGLE_AT_ZAXIS)
  169. {
  170. OnStageStopTubeAngleAtZxis();
  171. }
  172. else if (name == CALIBRATION_STOP_TUBE_HEIGHT_AT_ZAXIS)
  173. {
  174. OnStageStopTubeHeightAtZAxis();
  175. }
  176. else if (name == CALIBRATION_MOVE_TUBE_HEIGHT_AS_PHYSICAL)
  177. {
  178. OnStageMoveTubeHeightAsPhysical();
  179. }
  180. else if (name == CALIBRATION_ROTATE_TUBE_ANGLE_AS_PHYSICAL)
  181. {
  182. OnStageRotateTubeAngleAsPhysical();
  183. }
  184. else if (name == CALIBRATION_SWITCH_SVO_STATUS)
  185. {
  186. OnStageSwitchSvoStatus();
  187. }
  188. else if (name == CALIBRATION_AUTO_CALIBRATION_AD_START)
  189. {
  190. OnStageAutoCalibrationADStart();
  191. }
  192. else if (name == CALIBRATION_AUTO_CALIBRATION_AD_NEXT)
  193. {
  194. OnStageAutoCalibrationADNext();
  195. }
  196. else if (name == CALIBRATION_AUTO_CALIBRATION_AD_END)
  197. {
  198. OnStageAutoCalibrationADEnd();
  199. }
  200. else if (name == CALIBRATION_MOVE_TUBE_HORIZONTAL)
  201. {
  202. OnStageMoveTubeHorizontal();
  203. }
  204. else if (name == CALIBRATION_GET_TUBE_HORIZONTAL_AD)
  205. {
  206. OnStageGetTubeHorizontalAD();
  207. }
  208. else if (name == CALIBRATION_GET_TUBE_HORIZONTAL_ENCODER)
  209. {
  210. OnStageGetTubeHorizontalEncoder();
  211. }
  212. else if (name == CALIBRATION_GET_DETECTOR_HORIZONTAL_AD)
  213. {
  214. OnStageGetDetectorHorizontalAD();
  215. }
  216. }
  217. void CalibrationModel::OnStageMoveTubeHeight()
  218. {
  219. if (m_tubeLineMotionSwitch)
  220. {
  221. m_tubeLineMotionSwitch->OutputSignal(false);
  222. }
  223. m_tubeHeight->Move(
  224. m_stageArgs->TubeHeightMoveDirection,
  225. m_stageArgs->TubeHeightMoveStep,
  226. m_tubeHeightMotionPeriod
  227. );
  228. }
  229. void CalibrationModel::OnStageRotateTubeAngle()
  230. {
  231. m_tubeAngle->Rotate(
  232. m_stageArgs->TubeAngleRotateDirection,
  233. m_stageArgs->TubeAngleRotateStep,
  234. m_tubeAngleMotionPeriod);
  235. }
  236. void CalibrationModel::OnStageMoveTubeHeightAsPhysical()
  237. {
  238. if (m_tubeLineMotionSwitch)
  239. {
  240. m_tubeLineMotionSwitch->OutputSignal(false);
  241. }
  242. auto step = m_coordinates->ConvertMotorStepValue(m_tubeHeight->Name(), m_stageArgs->TubeHeightMovePhysical);
  243. m_tubeHeight->Move(
  244. m_stageArgs->TubeHeightMoveDirection,
  245. step,
  246. m_tubeHeightMotionPeriod
  247. );
  248. }
  249. void CalibrationModel::OnStageRotateTubeAngleAsPhysical()
  250. {
  251. auto step = m_coordinates->ConvertMotorStepValue(m_tubeAngle->Name(), m_stageArgs->TubeAngleRotatePhysical);
  252. m_tubeAngle->Rotate(
  253. m_stageArgs->TubeAngleRotateDirection,
  254. step,
  255. m_tubeAngleMotionPeriod);
  256. }
  257. void CalibrationModel::OnStageGetTubeAngleAD()
  258. {
  259. auto ad = m_tubeAngleAD->GetCurrentADValue();
  260. m_stageArgs->TubeAngleAD = ad;
  261. }
  262. void CalibrationModel::OnStageGetTubeHeightAD()
  263. {
  264. auto ad = m_tubeHeightAD->GetCurrentADValue();
  265. m_stageArgs->TubeHeightAD = ad;
  266. }
  267. void CalibrationModel::OnStageGetDetectorHeightAD()
  268. {
  269. auto ad = m_detectorAD->GetCurrentADValue();
  270. m_stageArgs->DetectorHeightAD = ad;
  271. }
  272. void CalibrationModel::OnStageGetTubeAngleEncoder()
  273. {
  274. auto encoder = m_tubeAngleEncoder->GetCurrentEncoderValue();
  275. m_stageArgs->TubeAngleEncoder = encoder;
  276. }
  277. void CalibrationModel::OnStageGetTubeHeightEncoder()
  278. {
  279. auto encoder = m_tubeHeightEncoder->GetCurrentEncoderValue();
  280. m_stageArgs->TubeHeightEncoder= encoder;
  281. }
  282. void CalibrationModel::OnStageGetDetectorHeightEncoder()
  283. {
  284. auto encoder = m_detectorHeightEncoder->GetCurrentEncoderValue();
  285. m_stageArgs->DetectorHeightEncoder = encoder;
  286. }
  287. void CalibrationModel::OnStageActiveTubeAngleZAxisClear()
  288. {
  289. m_tubeAngleEncoder->BindZAxis();
  290. m_tubeAngleEncoder->ActiveAutoNotifyWhenZClear(true);
  291. }
  292. void CalibrationModel::OnStageCancelTubeAngleZAxisClear()
  293. {
  294. m_tubeAngleEncoder->UnBindZAxis();
  295. m_tubeAngleEncoder->ActiveAutoNotifyWhenZClear(false);
  296. }
  297. void CalibrationModel::OnStageActiveTubeHeightZAxisClear()
  298. {
  299. m_tubeHeightEncoder->BindZAxis();
  300. m_tubeHeightEncoder->ActiveAutoNotifyWhenZClear(true);
  301. }
  302. void CalibrationModel::OnStageCancelTubeHeightZAxisClear()
  303. {
  304. m_tubeHeightEncoder->UnBindZAxis();
  305. m_tubeHeightEncoder->ActiveAutoNotifyWhenZClear(false);
  306. }
  307. void CalibrationModel::OnStageRotateTubeAngleToZAxis()
  308. {
  309. m_tubeAngleEncoder->BindZAxis();
  310. m_tubeAngleEncoder->ActiveAutoNotifyWhenZClear(true);
  311. int direction = m_tubeRotateAxisPositiveDirection > 0 ? 1 : -1;
  312. direction = m_tubeRotateLandmarkDirection > 0 ? direction : (-1 * direction);
  313. direction = direction * -1;
  314. m_tubeAngle->Rotate(direction, -1, m_tubeAngleMotionPeriod);
  315. }
  316. void CalibrationModel::OnStageMoveTubeHeightToZAxis()
  317. {
  318. m_tubeHeightEncoder->BindZAxis();
  319. m_tubeHeightEncoder->ActiveAutoNotifyWhenZClear(true);
  320. int direction = m_tubeHeightAxisPositiveDirection > 0 ? 1 : -1;
  321. //direction = m_tubeHeightLandmarkDirection > 0 ? direction : (-1 * direction);
  322. direction = m_tubeHeightLandmarkDirection > 0 ? (-1 * direction) : direction;
  323. direction = direction * -1;
  324. m_tubeHeight->Move(direction, -1, m_tubeHeightMotionPeriod);
  325. }
  326. void CalibrationModel::OnStageStopTubeAngleAtZxis()
  327. {
  328. m_tubeAngle->StopRotation();
  329. }
  330. void CalibrationModel::OnStageStopTubeHeightAtZAxis()
  331. {
  332. m_tubeHeight->StopMove();
  333. }
  334. void CalibrationModel::OnStageSwitchSvoStatus()
  335. {
  336. int targetStatus = m_stageArgs->SvoStatus;
  337. if (m_stageArgs->AutoCalADDof == 0)
  338. {
  339. m_tubeAngle->SetServoStatus(targetStatus);
  340. }
  341. else if (m_stageArgs->AutoCalADDof == 1)
  342. {
  343. m_tubeHeight->SetServoStatus(targetStatus);
  344. }
  345. }
  346. void CalibrationModel::OnStageAutoCalibrationADStart()
  347. {
  348. m_stageArgs->AutoCalADPhysicals.clear();
  349. m_stageArgs->AutoCalADADs.clear();
  350. auto hlim = m_stageArgs->AutoCalADHighLimit;
  351. auto llim = m_stageArgs->AutoCalADLowLimit;
  352. m_autoCalADPhysicalEachStep = (hlim - llim) / (m_stageArgs->AutoCalADTotalStep);
  353. //Move to start pos(high limit)
  354. float current = m_stageArgs->AutoCalADCurrentPhysical;
  355. float target = m_stageArgs->AutoCalADHighLimit;
  356. MoveMech(m_stageArgs->AutoCalADDof,current, target);
  357. }
  358. void CalibrationModel::OnStageAutoCalibrationADNext()
  359. {
  360. m_stageArgs->AutoCalADPhysicals.push_back(m_stageArgs->AutoCalADHighLimit - (m_autoCalADPhysicalEachStep * m_stageArgs->AutoCalADCurrentStep));
  361. m_stageArgs->AutoCalADADs.push_back(GetAD(m_stageArgs->AutoCalADDof) * 1.0f);
  362. //Move to next pos
  363. float current = m_stageArgs->AutoCalADHighLimit - (m_autoCalADPhysicalEachStep * m_stageArgs->AutoCalADCurrentStep);
  364. float target = current - m_autoCalADPhysicalEachStep;
  365. MoveMech(m_stageArgs->AutoCalADDof,current, target);
  366. }
  367. void CalibrationModel::OnStageAutoCalibrationADEnd()
  368. {
  369. }
  370. int CalibrationModel::GetAD(int dof)
  371. {
  372. if (dof == 0)
  373. {
  374. return m_tubeAngleAD->GetCurrentADValue();
  375. }
  376. else if (dof == 1)
  377. {
  378. return m_tubeHeightAD->GetCurrentADValue();
  379. }
  380. return 0;
  381. }
  382. void CalibrationModel::MoveMech(int dof, float current, float target)
  383. {
  384. if (dof == 0)
  385. {
  386. RotateTubeAngle(current, target);
  387. }
  388. else if (dof == 1)
  389. {
  390. MoveTubeHeight(current, target);
  391. }
  392. }
  393. void CalibrationModel::RotateTubeAngle(float current, float target)
  394. {
  395. float offset = abs(current - target);
  396. auto step = m_coordinates->ConvertMotorStepValue(m_tubeAngle->Name(), offset);
  397. auto direction = JudgeDirectionInTubeAngleAxis(current, target);
  398. m_tubeAngle->Rotate(
  399. direction,
  400. step,
  401. m_tubeAngleMotionPeriod);
  402. }
  403. void CalibrationModel::MoveTubeHeight(float current, float target)
  404. {
  405. float offset = abs(current - target);
  406. auto step = m_coordinates->ConvertMotorStepValue(m_tubeHeight->Name(), offset);
  407. auto direction = JudgeDirectionInTubeHeightAxis(current, target);
  408. m_tubeHeight->Move(
  409. direction,
  410. step,
  411. m_tubeHeightMotionPeriod
  412. );
  413. }
  414. int CalibrationModel::JudgeDirectionInTubeHeightAxis(float current, float target)
  415. {
  416. auto positive = ConfigurerMotion::GetTubeHeightAxisPositiveDirection();
  417. auto direction = positive > 0 ? 1 : -1;
  418. if (current > target)
  419. {
  420. return 1 * direction;
  421. }
  422. if (current < target)
  423. {
  424. return -1 * direction;
  425. }
  426. return 0;
  427. }
  428. int CalibrationModel::JudgeDirectionInTubeAngleAxis(float current, float target)
  429. {
  430. auto positive = ConfigurerMotion::GetTubeRotateAxisPositiveDirection();
  431. auto direction = positive > 0 ? 1 : -1;
  432. if (current > target)
  433. {
  434. return -1 * direction;
  435. }
  436. if (current < target)
  437. {
  438. return 1 * direction;
  439. }
  440. return 0;
  441. }
  442. void CalibrationModel::SwitchScanningComponents(int nSwitch)
  443. {
  444. }
  445. void CalibrationModel::SwitchWorkstation(string ws)
  446. {
  447. m_CurWS = ws;
  448. }
  449. void CalibrationModel::OnStageMoveTubeHorizontal()
  450. {
  451. if (m_tubeLineMotionSwitch)
  452. {
  453. m_tubeLineMotionSwitch->OutputSignal(true);
  454. }
  455. m_motorTubeHorizontal->Move(
  456. m_stageArgs->TubeHorizontalMoveDirection,
  457. m_stageArgs->TubeHorizontalMoveStep,
  458. m_tubeHorizontalMotionPeriod
  459. );
  460. }
  461. void CalibrationModel::OnStageGetTubeHorizontalAD()
  462. {
  463. auto ad = m_adTubeHorizontal->GetCurrentADValue();
  464. m_stageArgs->TubeHorizontalAD = ad;
  465. }
  466. void CalibrationModel::OnStageGetTubeHorizontalEncoder()
  467. {
  468. auto encoder = m_encoderTubeHorizontal->GetCurrentEncoderValue();
  469. m_stageArgs->TubeHorizontalEncoder = encoder;
  470. }
  471. void CalibrationModel::OnStageGetDetectorHorizontalAD()
  472. {
  473. auto ad = m_detectorHorizontalAD->GetCurrentADValue();
  474. m_stageArgs->DetectorHorizontalAD = ad;
  475. }