carrier_base.cpp 29 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853
  1. //
  2. // Created by huli on 2020/7/20.
  3. //
  4. #include "carrier_base.h"
  5. #include "dispatch_communication.h"
  6. /*
  7. Carrier_base::Carrier_base()
  8. {
  9. m_carrier_status = E_UNKNOW;
  10. m_carrier_id = -1;
  11. m_request_x = 0; //搬运器坐标x轴, 中跑车控制横向移动
  12. m_request_y = 0; //搬运器坐标y轴, 小跑车控制纵向移动
  13. m_request_z = 0; //搬运器坐标z轴, 电梯控制上下移动
  14. m_request_space_id = 0; //搬运器空间位置的id.
  15. m_request_y1 = 0; //搬运器坐标y1轴, 小跑车控制纵向移动(前轮抓杆)
  16. m_request_y2 = 0; //搬运器坐标y2轴, 小跑车控制纵向移动(后轮抓杆)
  17. m_request_wheelbase = 0; //搬运器小跑车的抓车杆前后轮距.
  18. m_request_clamp_motion = E_CLAMP_NO_ACTION; //小跑车夹车杆. 0=无动作,1=夹紧,2=松开
  19. m_request_joint_motion = E_JOINT_NO_ACTION; //电梯与X轴的横向轨道对接,0=无动作,
  20. m_respons_status = RESPONS_WORKING;
  21. m_respons_x = 0; //搬运器坐标x轴, 中跑车控制横向移动
  22. m_respons_y = 0; //搬运器坐标y轴, 小跑车控制纵向移动
  23. m_respons_z = 0; //搬运器坐标z轴, 电梯控制上下移动
  24. m_respons_space_id = 0; //搬运器空间位置的id.
  25. m_respons_y1 = 0; //搬运器坐标y1轴, 小跑车控制纵向移动(前轮抓杆)
  26. m_respons_y2 = 0; //搬运器坐标y2轴, 小跑车控制纵向移动(后轮抓杆)
  27. m_respons_wheelbase = 0; //搬运器小跑车的抓车杆前后轮距.
  28. m_respons_clamp_motion = E_CLAMP_NO_ACTION; //小跑车夹车杆. 0=无动作,1=夹紧,2=松开
  29. m_respons_joint_motion = E_JOINT_NO_ACTION; //电梯与X轴的横向轨道对接,0=无动作,
  30. m_status_updata_time = std::chrono::system_clock::now();
  31. m_last_heartbeat = 0;
  32. m_device_status = DEVICE_UNKNOWN;
  33. m_load_status = LOAD_UNKNOWN;
  34. m_actual_x = 0; //搬运器坐标x轴, 中跑车控制横向移动
  35. m_actual_y = 0; //搬运器坐标y轴, 小跑车控制纵向移动
  36. m_actual_z = 0; //搬运器坐标z轴, 电梯控制上下移动
  37. m_actual_y1 = 0; //搬运器坐标y1轴, 小跑车控制纵向移动(前轮抓杆)
  38. m_actual_y2 = 0; //搬运器坐标y2轴, 小跑车控制纵向移动(后轮抓杆)
  39. m_actual_clamp_motion1 = E_CLAMP_NO_ACTION; //小跑车夹车杆. 0=无动作,1=夹紧,2=松开
  40. m_actual_clamp_motion2 = E_CLAMP_NO_ACTION; //小跑车夹车杆. 0=无动作,1=夹紧,2=松开
  41. m_actual_joint_motion1 = E_JOINT_NO_ACTION; //电梯与X轴的横向轨道对接,0=无动作,
  42. m_actual_joint_motion2 = E_JOINT_NO_ACTION; //电梯与X轴的横向轨道对接,0=无动作,
  43. m_actual_small_sports_car_motion = E_SMALL_SPORTS_NO_ACTION; //小跑车的位置,是否在中跑车上面
  44. memset(m_carrier_error_code, 0, 50); //搬运器设备的报警信息位
  45. memset(m_carrier_warning_code, 0, 50); //升降机设备的报警信息位
  46. mp_execute_thread = NULL;
  47. }
  48. Carrier_base::~Carrier_base()
  49. {
  50. carrier_base_uninit();
  51. }
  52. //搬运器 初始化
  53. Error_manager Carrier_base::carrier_base_init()
  54. {
  55. // 线程默认开启
  56. m_execute_condition.reset(false, true, false);
  57. mp_execute_thread = new std::thread(&Carrier_base::execute_thread_fun, this);
  58. m_carrier_status = E_READY;
  59. return Error_code::SUCCESS;
  60. }
  61. //搬运器 反初始化
  62. Error_manager Carrier_base::carrier_base_uninit()
  63. {
  64. if (mp_execute_thread)
  65. {
  66. m_execute_condition.kill_all();
  67. }
  68. if (mp_execute_thread)
  69. {
  70. mp_execute_thread->join();
  71. delete mp_execute_thread;
  72. mp_execute_thread = NULL;
  73. }
  74. m_carrier_status = E_UNKNOW;
  75. return Error_code::SUCCESS;
  76. }
  77. //处理一级任务, 普通任务
  78. Error_manager Carrier_base::execute_one_level_task(std::shared_ptr<Carrier_task> p_carrier_task)
  79. {
  80. Error_manager t_error;
  81. Error_manager t_result;
  82. //检查指针
  83. if (p_carrier_task == NULL) {
  84. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  85. "Carrier_base::execute_one_level_task failed, POINTER_IS_NULL");
  86. }
  87. //检查任务类型,
  88. if (p_carrier_task->get_task_type() != CARRIER_TASK)
  89. {
  90. return Error_manager(Error_code::CARRIER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
  91. "Carrier_base::execute_one_level_task get_task_type() != CARRIER_TASK ");
  92. }
  93. //检查接收方的状态
  94. t_error = check_one_level_task();
  95. if ( t_error != SUCCESS )
  96. {
  97. t_result.compare_and_cover_error(t_error);
  98. }
  99. else
  100. {
  101. //接受任务,并将任务的状态改为TASK_SIGNED已签收
  102. mp_carrier_one_level_task = p_carrier_task;
  103. mp_carrier_one_level_task->set_task_statu(TASK_SIGNED);
  104. //这里不用检查任务内容, 直接下发给底层设备
  105. //启动定位管理模块,的核心工作线程
  106. m_carrier_status = E_ONE_LEVEL_WORK;
  107. m_execute_condition.notify_all(true);
  108. //通知 thread_work 子线程启动。
  109. //将任务的状态改为 TASK_WORKING 处理中
  110. //20200225, 在线程真正的执行的时候,才改为工作中, (执行线程可能会先处理更高优先级的任务单)
  111. // mp_carrier_one_level_task->set_task_statu(TASK_WORKING);
  112. }
  113. if ( t_result != Error_code::SUCCESS )
  114. {
  115. return t_result;
  116. }
  117. return Error_code::SUCCESS;
  118. }
  119. //处理二级任务, 插队任务
  120. Error_manager Carrier_base::execute_two_level_task(std::shared_ptr<Carrier_task> p_carrier_task)
  121. {
  122. Error_manager t_error;
  123. Error_manager t_result;
  124. //检查指针
  125. if (p_carrier_task == NULL) {
  126. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  127. "Carrier_base::execute_two_level_task failed, POINTER_IS_NULL");
  128. }
  129. //检查任务类型,
  130. if (p_carrier_task->get_task_type() != CARRIER_TASK)
  131. {
  132. return Error_manager(Error_code::CARRIER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
  133. "Carrier_base::execute_two_level_task get_task_type() != CARRIER_TASK ");
  134. }
  135. //检查接收方的状态
  136. t_error = check_two_level_task();
  137. if ( t_error != SUCCESS )
  138. {
  139. t_result.compare_and_cover_error(t_error);
  140. }
  141. else
  142. {
  143. //接受任务,并将任务的状态改为TASK_SIGNED已签收
  144. mp_carrier_two_level_task = p_carrier_task;
  145. mp_carrier_two_level_task->set_task_statu(TASK_SIGNED);
  146. //这里不用检查任务内容, 直接下发给底层设备
  147. //启动定位管理模块,的核心工作线程
  148. m_carrier_status = E_TWO_LEVEL_WORK;
  149. m_execute_condition.notify_all(true);
  150. //通知 thread_work 子线程启动。
  151. //将任务的状态改为 TASK_WORKING 处理中
  152. //20200225, 在线程真正的执行的时候,才改为工作中, (执行线程可能会先处理更高优先级的任务单)
  153. // mp_carrier_two_level_task->set_task_statu(TASK_WORKING);
  154. }
  155. if ( t_result != Error_code::SUCCESS )
  156. {
  157. return t_result;
  158. }
  159. return Error_code::SUCCESS;
  160. }
  161. //处理三级任务, 核心任务
  162. Error_manager Carrier_base::execute_three_level_task(std::shared_ptr<Carrier_task> p_carrier_task)
  163. {
  164. Error_manager t_error;
  165. Error_manager t_result;
  166. //检查指针
  167. if (p_carrier_task == NULL) {
  168. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  169. "Carrier_base::execute_three_level_task failed, POINTER_IS_NULL");
  170. }
  171. //检查任务类型,
  172. if (p_carrier_task->get_task_type() != CARRIER_TASK)
  173. {
  174. return Error_manager(Error_code::CARRIER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
  175. "Carrier_base::execute_three_level_task get_task_type() != CARRIER_TASK ");
  176. }
  177. //检查接收方的状态
  178. t_error = check_three_level_task();
  179. if ( t_error != SUCCESS )
  180. {
  181. t_result.compare_and_cover_error(t_error);
  182. }
  183. else
  184. {
  185. //接受任务,并将任务的状态改为TASK_SIGNED已签收
  186. mp_carrier_three_level_task = p_carrier_task;
  187. mp_carrier_three_level_task->set_task_statu(TASK_SIGNED);
  188. //这里不用检查任务内容, 直接下发给底层设备
  189. //启动定位管理模块,的核心工作线程
  190. m_carrier_status = E_THREE_LEVEL_WORK;
  191. m_execute_condition.notify_all(true);
  192. //通知 thread_work 子线程启动。
  193. //将任务的状态改为 TASK_WORKING 处理中
  194. //20200225, 在线程真正的执行的时候,才改为工作中, (执行线程可能会先处理更高优先级的任务单)
  195. // mp_carrier_three_level_task->set_task_statu(TASK_WORKING);
  196. }
  197. if ( t_result != Error_code::SUCCESS )
  198. {
  199. return t_result;
  200. }
  201. return Error_code::SUCCESS;
  202. }
  203. //检查状态,是否正常运行
  204. Error_manager Carrier_base::check_status()
  205. {
  206. if ( m_carrier_status == E_READY )
  207. {
  208. return Error_code::SUCCESS;
  209. }
  210. else if ( m_carrier_status >= E_BUSY && m_carrier_status <= E_THREE_LEVEL_OVER)
  211. {
  212. return Error_manager(Error_code::CARRIER_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
  213. " Carrier_base::check_status is busy ");
  214. }
  215. else
  216. {
  217. return Error_manager(Error_code::CARRIER_STATUS_ERROR, Error_level::MINOR_ERROR,
  218. " Carrier_base::check_status error ");
  219. }
  220. return Error_code::SUCCESS;
  221. }
  222. //判断能否执行一级任务
  223. Error_manager Carrier_base::check_one_level_task()
  224. {
  225. if ( m_carrier_status == E_READY )
  226. {
  227. return Error_code::SUCCESS;
  228. }
  229. else if ( m_carrier_status >= E_BUSY && m_carrier_status <= E_THREE_LEVEL_OVER)
  230. {
  231. return Error_manager(Error_code::CARRIER_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
  232. " Carrier_base::check_status is busy ");
  233. }
  234. else
  235. {
  236. return Error_manager(Error_code::CARRIER_STATUS_ERROR, Error_level::MINOR_ERROR,
  237. " Carrier_base::check_status error ");
  238. }
  239. return Error_code::SUCCESS;
  240. }
  241. //判断能否执行二级任务
  242. Error_manager Carrier_base::check_two_level_task()
  243. {
  244. if ( m_carrier_status == E_READY ||
  245. m_carrier_status == E_ONE_LEVEL_WORK ||
  246. m_carrier_status == E_ONE_LEVEL_OVER )
  247. {
  248. return Error_code::SUCCESS;
  249. }
  250. else if ( m_carrier_status >= E_TWO_LEVEL_WORK && m_carrier_status <= E_THREE_LEVEL_OVER)
  251. {
  252. return Error_manager(Error_code::CARRIER_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
  253. " Carrier_base::check_status is busy ");
  254. }
  255. else
  256. {
  257. return Error_manager(Error_code::CARRIER_STATUS_ERROR, Error_level::MINOR_ERROR,
  258. " Carrier_base::check_status error ");
  259. }
  260. return Error_code::SUCCESS;
  261. }
  262. //判断能否执行三级任务
  263. Error_manager Carrier_base::check_three_level_task()
  264. {
  265. if ( m_carrier_status == E_READY ||
  266. m_carrier_status == E_ONE_LEVEL_WORK ||
  267. m_carrier_status == E_ONE_LEVEL_OVER )
  268. {
  269. return Error_code::SUCCESS;
  270. }
  271. else if ( m_carrier_status >= E_TWO_LEVEL_WORK && m_carrier_status <= E_THREE_LEVEL_OVER)
  272. {
  273. return Error_manager(Error_code::CARRIER_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
  274. " Carrier_base::check_status is busy ");
  275. }
  276. else
  277. {
  278. return Error_manager(Error_code::CARRIER_STATUS_ERROR, Error_level::MINOR_ERROR,
  279. " Carrier_base::check_status error ");
  280. }
  281. return Error_code::SUCCESS;
  282. }
  283. //结束任务单,里面会根据任务的故障等级修正 任务单的状态
  284. Error_manager Carrier_base::end_task(std::shared_ptr<Carrier_task> carrier_task)
  285. {
  286. LOG(INFO) << " ---Carrier_base::end_task run---"<< this;
  287. // carrier_task->m_respons_status = (Carrier_task::Respons_status)m_respons_status;
  288. //注:这里只修改任务单的状态, 搬运器的状态不管
  289. //在结束任务单时,将雷达任务状态改为 TASK_OVER 已结束
  290. //判断任务单的错误等级,
  291. if ( carrier_task->get_task_error_manager().get_error_level() < Error_level::MINOR_ERROR)
  292. {
  293. //强制改为TASK_OVER,不管它当前在做什么。
  294. carrier_task->set_task_statu(TASK_OVER);
  295. }
  296. else
  297. {
  298. //强制改为 TASK_ERROR,不管它当前在做什么。
  299. carrier_task->set_task_statu(TASK_ERROR);
  300. }
  301. return Error_code::SUCCESS;
  302. }
  303. //取消任务单,由发送方提前取消任务单
  304. Error_manager Carrier_base::cancel_task(std::shared_ptr<Carrier_task> carrier_task)
  305. {
  306. //找到对应的任务单
  307. if ( carrier_task->get_task_id() == mp_carrier_one_level_task->get_task_id() )
  308. {
  309. if ( m_carrier_status == E_ONE_LEVEL_WORK || m_carrier_status == E_ONE_LEVEL_OVER )
  310. {
  311. //如果正在执行一级任务, 那么取消当前指令, 然后降级
  312. m_execute_condition.notify_all(false);
  313. //确保内部线程已经停下
  314. while (m_execute_condition.is_working())
  315. {
  316. }
  317. cancel_command();
  318. mp_carrier_one_level_task.reset();
  319. m_carrier_status = E_READY;
  320. m_execute_condition.notify_all(true);
  321. }
  322. else
  323. {
  324. //否则直接销毁任务单
  325. mp_carrier_one_level_task.reset();
  326. }
  327. }
  328. else if ( carrier_task->get_task_id() == mp_carrier_two_level_task->get_task_id() )
  329. {
  330. if ( m_carrier_status == E_TWO_LEVEL_WORK || m_carrier_status == E_TWO_LEVEL_OVER )
  331. {
  332. //如果正在执行二级任务, 那么取消当前指令, 然后降级
  333. m_execute_condition.notify_all(false);
  334. //确保内部线程已经停下
  335. while (m_execute_condition.is_working())
  336. {
  337. }
  338. cancel_command();
  339. mp_carrier_two_level_task.reset();
  340. m_carrier_status = E_ONE_LEVEL_WORK;
  341. m_execute_condition.notify_all(true);
  342. }
  343. else
  344. {
  345. //否则直接销毁任务单
  346. mp_carrier_two_level_task.reset();
  347. }
  348. }
  349. else if ( carrier_task->get_task_id() == mp_carrier_three_level_task->get_task_id() )
  350. {
  351. if ( m_carrier_status == E_THREE_LEVEL_WORK || m_carrier_status == E_THREE_LEVEL_OVER )
  352. {
  353. //如果正在执行三级任务, 那么取消当前指令, 然后降级
  354. m_execute_condition.notify_all(false);
  355. //确保内部线程已经停下
  356. while (m_execute_condition.is_working())
  357. {
  358. }
  359. cancel_command();
  360. mp_carrier_three_level_task.reset();
  361. m_carrier_status = E_TWO_LEVEL_WORK;
  362. m_execute_condition.notify_all(true);
  363. }
  364. else
  365. {
  366. //否则直接销毁任务单
  367. mp_carrier_two_level_task.reset();
  368. }
  369. }
  370. else
  371. {
  372. return Error_manager(Error_code::CARRIER_TASK_NOTHINGNESS, Error_level::MINOR_ERROR,
  373. " carrier_task->get_task_id() is nothingness ");
  374. }
  375. carrier_task->set_task_statu(TASK_DEAD);
  376. return Error_code::SUCCESS;
  377. }
  378. //判断是否为待机,如果已经准备好,则可以执行任务。
  379. bool Carrier_base::is_ready()
  380. {
  381. return (m_carrier_status == E_READY);
  382. }
  383. Carrier_base::Carrier_status Carrier_base::get_carrier_status()
  384. {
  385. return m_carrier_status;
  386. }
  387. //执行外界任务的执行函数
  388. void Carrier_base::execute_thread_fun()
  389. {
  390. LOG(INFO) << " execute_thread_fun start "<< this;
  391. // while (1);
  392. Error_manager t_error;
  393. while (m_execute_condition.is_alive())
  394. {
  395. m_execute_condition.wait();
  396. if ( m_execute_condition.is_alive() )
  397. {
  398. std::this_thread::sleep_for(std::chrono::microseconds(1));
  399. std::this_thread::yield();
  400. switch ( (Carrier_status)m_carrier_status )
  401. {
  402. //核心任务, (三级任务)
  403. case E_THREE_LEVEL_WORK:
  404. {
  405. if ( mp_carrier_three_level_task.get() != NULL )
  406. {
  407. mp_carrier_three_level_task->set_task_statu(TASK_WORKING);
  408. //执行三级任务
  409. std::unique_lock<std::mutex> t_lock(mp_carrier_three_level_task->m_lock);
  410. m_request_key = mp_carrier_three_level_task->m_request_key;
  411. m_request_x = mp_carrier_three_level_task->m_request_x;
  412. m_request_y = mp_carrier_three_level_task->m_request_y;
  413. m_request_z = mp_carrier_three_level_task->m_request_z;
  414. m_request_space_id = mp_carrier_three_level_task->m_request_space_id;
  415. m_request_y1 = mp_carrier_three_level_task->m_request_y1;
  416. m_request_y2 = mp_carrier_three_level_task->m_request_y2;
  417. m_request_wheelbase = mp_carrier_three_level_task->m_request_wheelbase;
  418. m_request_clamp_motion = (Carrier_base::Clamp_motion)mp_carrier_three_level_task->m_request_clamp_motion;
  419. m_request_joint_motion = (Carrier_base::Joint_motion)mp_carrier_three_level_task->m_request_joint_motion;
  420. //更新通信
  421. update_device_communication();
  422. //检查plc的答复状态
  423. if ( m_respons_key == m_request_key && m_respons_status != RESPONS_WORKING )
  424. {
  425. mp_carrier_three_level_task->m_respons_key = m_respons_key;
  426. mp_carrier_three_level_task->m_respons_status = (Carrier_task::Respons_status)m_respons_status;
  427. mp_carrier_three_level_task->m_respons_x = m_respons_x;
  428. mp_carrier_three_level_task->m_respons_y = m_respons_y;
  429. mp_carrier_three_level_task->m_respons_z = m_respons_z;
  430. mp_carrier_three_level_task->m_respons_space_id = m_respons_space_id;
  431. mp_carrier_three_level_task->m_respons_y1 = m_respons_y1;
  432. mp_carrier_three_level_task->m_respons_y2 = m_respons_y2;
  433. mp_carrier_three_level_task->m_respons_wheelbase = m_respons_wheelbase;
  434. mp_carrier_three_level_task->m_respons_clamp_motion = (Carrier_task::Clamp_motion)m_respons_clamp_motion;
  435. mp_carrier_three_level_task->m_respons_joint_motion = (Carrier_task::Joint_motion)m_respons_joint_motion;
  436. //如果故障,则添加错误码
  437. if ( m_respons_status == RESPONS_MINOR_ERROR || m_respons_status == RESPONS_CRITICAL_ERROR )
  438. {
  439. //添加错误码
  440. Error_manager t_error(CARRIER_RESPONS_ERROR, MINOR_ERROR, "m_respons_status is error");
  441. mp_carrier_three_level_task->set_task_error_manager(t_error);
  442. }
  443. end_task(mp_carrier_three_level_task);
  444. m_carrier_status = E_THREE_LEVEL_OVER;
  445. }
  446. else
  447. {
  448. //设备正常运行
  449. //延时1ms, snap7的通信效率偏慢, 不需要高频率检查
  450. std::this_thread::sleep_for(std::chrono::milliseconds(1));
  451. }
  452. //else继续等待,直到任务完成
  453. }
  454. else
  455. {
  456. //直接降级
  457. m_carrier_status = E_TWO_LEVEL_WORK;
  458. }
  459. break;
  460. }
  461. case E_THREE_LEVEL_OVER:
  462. {
  463. if ( mp_carrier_three_level_task.get() != NULL )
  464. {
  465. //检查任务状态,
  466. //在 E_THREE_LEVEL_WORK 里面, 已经 设为 TASK_OVER 了.
  467. //等待发送方的新指令, (发送方会把状态改为回收, 表示这个任务结束)
  468. if ( mp_carrier_three_level_task->get_task_statu() == TASK_WITHDRAW )
  469. {
  470. //这里会通知任务已经释放, 然后销毁任务单, 并降级
  471. mp_carrier_three_level_task->set_task_statu(TASK_FREE);
  472. mp_carrier_three_level_task.reset();
  473. m_carrier_status = E_TWO_LEVEL_WORK;
  474. }
  475. //任务单重新创建, 调度创建的新的任务,那么回去继续工作
  476. else if ( mp_carrier_three_level_task->get_task_statu() == TASK_CREATED )
  477. {
  478. mp_carrier_three_level_task->set_task_statu(TASK_SIGNED);
  479. m_carrier_status = E_THREE_LEVEL_WORK;
  480. }
  481. //else //保持不动, 直到发送方给定新的任务,
  482. }
  483. else
  484. {
  485. //直接降级
  486. m_carrier_status = E_TWO_LEVEL_WORK;
  487. }
  488. break;
  489. }
  490. case E_TWO_LEVEL_WORK:
  491. {
  492. if ( mp_carrier_two_level_task.get() != NULL )
  493. {
  494. mp_carrier_two_level_task->set_task_statu(TASK_WORKING);
  495. //执行二级任务
  496. std::unique_lock<std::mutex> t_lock(mp_carrier_two_level_task->m_lock);
  497. m_request_key = mp_carrier_two_level_task->m_request_key;
  498. m_request_x = mp_carrier_two_level_task->m_request_x;
  499. m_request_y = mp_carrier_two_level_task->m_request_y;
  500. m_request_z = mp_carrier_two_level_task->m_request_z;
  501. m_request_space_id = mp_carrier_two_level_task->m_request_space_id;
  502. m_request_y1 = mp_carrier_two_level_task->m_request_y1;
  503. m_request_y2 = mp_carrier_two_level_task->m_request_y2;
  504. m_request_wheelbase = mp_carrier_two_level_task->m_request_wheelbase;
  505. m_request_clamp_motion = (Carrier_base::Clamp_motion)mp_carrier_two_level_task->m_request_clamp_motion;
  506. m_request_joint_motion = (Carrier_base::Joint_motion)mp_carrier_two_level_task->m_request_joint_motion;
  507. //更新通信
  508. update_device_communication();
  509. //检查plc的答复状态
  510. if ( m_respons_key == m_request_key && m_respons_status != RESPONS_WORKING )
  511. {
  512. mp_carrier_two_level_task->m_respons_key = m_respons_key;
  513. mp_carrier_two_level_task->m_respons_status = (Carrier_task::Respons_status)m_respons_status;
  514. mp_carrier_two_level_task->m_respons_x = m_respons_x;
  515. mp_carrier_two_level_task->m_respons_y = m_respons_y;
  516. mp_carrier_two_level_task->m_respons_z = m_respons_z;
  517. mp_carrier_two_level_task->m_respons_space_id = m_respons_space_id;
  518. mp_carrier_two_level_task->m_respons_y1 = m_respons_y1;
  519. mp_carrier_two_level_task->m_respons_y2 = m_respons_y2;
  520. mp_carrier_two_level_task->m_respons_wheelbase = m_respons_wheelbase;
  521. mp_carrier_two_level_task->m_respons_clamp_motion = (Carrier_task::Clamp_motion)m_respons_clamp_motion;
  522. mp_carrier_two_level_task->m_respons_joint_motion = (Carrier_task::Joint_motion)m_respons_joint_motion;
  523. //如果故障,则添加错误码
  524. if ( m_respons_status == RESPONS_MINOR_ERROR || m_respons_status == RESPONS_CRITICAL_ERROR )
  525. {
  526. //添加错误码
  527. Error_manager t_error(CARRIER_RESPONS_ERROR, MINOR_ERROR, "m_respons_status is error");
  528. mp_carrier_two_level_task->set_task_error_manager(t_error);
  529. }
  530. end_task(mp_carrier_two_level_task);
  531. m_carrier_status = E_THREE_LEVEL_OVER;
  532. }
  533. else
  534. {
  535. //设备正常运行
  536. //延时1ms, snap7的通信效率偏慢, 不需要高频率检查
  537. std::this_thread::sleep_for(std::chrono::milliseconds(1));
  538. }
  539. //else继续等待,直到任务完成
  540. }
  541. else
  542. {
  543. //直接降级
  544. m_carrier_status = E_ONE_LEVEL_WORK;
  545. }
  546. break;
  547. }
  548. case E_TWO_LEVEL_OVER:
  549. {
  550. if ( mp_carrier_two_level_task.get() != NULL )
  551. {
  552. //检查任务状态,
  553. //在 E_TWO_LEVEL_WORK 里面, 已经 设为 TASK_OVER 了.
  554. //等待发送方的新指令, (发送方会把状态改为回收, 表示这个任务结束)
  555. if ( mp_carrier_two_level_task->get_task_statu() == TASK_WITHDRAW )
  556. {
  557. //这里会通知任务已经释放, 然后销毁任务单, 并降级
  558. mp_carrier_two_level_task->set_task_statu(TASK_FREE);
  559. mp_carrier_two_level_task.reset();
  560. m_carrier_status = E_ONE_LEVEL_WORK;
  561. }
  562. //任务单重新创建, 调度创建的新的任务,那么回去继续工作
  563. else if ( mp_carrier_two_level_task->get_task_statu() == TASK_CREATED )
  564. {
  565. mp_carrier_two_level_task->set_task_statu(TASK_SIGNED);
  566. m_carrier_status = E_TWO_LEVEL_WORK;
  567. }
  568. //else //保持不动, 直到发送方给定新的任务,
  569. }
  570. else
  571. {
  572. //直接降级
  573. m_carrier_status = E_ONE_LEVEL_WORK;
  574. }
  575. break;
  576. }
  577. case E_ONE_LEVEL_WORK:
  578. {
  579. if ( mp_carrier_one_level_task.get() != NULL )
  580. {
  581. mp_carrier_one_level_task->set_task_statu(TASK_WORKING);
  582. //执行一级任务,
  583. std::unique_lock<std::mutex> t_lock(mp_carrier_one_level_task->m_lock);
  584. m_request_key = mp_carrier_one_level_task->m_request_key;
  585. m_request_x = mp_carrier_one_level_task->m_request_x;
  586. m_request_y = mp_carrier_one_level_task->m_request_y;
  587. m_request_z = mp_carrier_one_level_task->m_request_z;
  588. m_request_space_id = mp_carrier_one_level_task->m_request_space_id;
  589. m_request_y1 = mp_carrier_one_level_task->m_request_y1;
  590. m_request_y2 = mp_carrier_one_level_task->m_request_y2;
  591. m_request_wheelbase = mp_carrier_one_level_task->m_request_wheelbase;
  592. m_request_clamp_motion = (Carrier_base::Clamp_motion)mp_carrier_one_level_task->m_request_clamp_motion;
  593. m_request_joint_motion = (Carrier_base::Joint_motion)mp_carrier_one_level_task->m_request_joint_motion;
  594. //更新通信
  595. update_device_communication();
  596. //检查plc的答复状态
  597. if ( m_respons_key == m_request_key && m_respons_status != RESPONS_WORKING )
  598. {
  599. mp_carrier_one_level_task->m_respons_x = m_respons_x;
  600. mp_carrier_one_level_task->m_respons_y = m_respons_y;
  601. mp_carrier_one_level_task->m_respons_z = m_respons_z;
  602. mp_carrier_one_level_task->m_respons_space_id = m_respons_space_id;
  603. mp_carrier_one_level_task->m_respons_y1 = m_respons_y1;
  604. mp_carrier_one_level_task->m_respons_y2 = m_respons_y2;
  605. mp_carrier_one_level_task->m_respons_wheelbase = m_respons_wheelbase;
  606. mp_carrier_one_level_task->m_respons_clamp_motion = (Carrier_task::Clamp_motion)m_respons_clamp_motion;
  607. mp_carrier_one_level_task->m_respons_joint_motion = (Carrier_task::Joint_motion)m_respons_joint_motion;
  608. //如果故障,则添加错误码
  609. if ( m_respons_status == RESPONS_MINOR_ERROR || m_respons_status == RESPONS_CRITICAL_ERROR )
  610. {
  611. //添加错误码
  612. Error_manager t_error(CARRIER_RESPONS_ERROR, MINOR_ERROR, "m_respons_status is error");
  613. mp_carrier_one_level_task->set_task_error_manager(t_error);
  614. }
  615. end_task(mp_carrier_one_level_task);
  616. m_carrier_status = E_ONE_LEVEL_OVER;
  617. }
  618. else
  619. {
  620. //设备正常运行
  621. //延时1ms, snap7的通信效率偏慢, 不需要高频率检查
  622. std::this_thread::sleep_for(std::chrono::milliseconds(1));
  623. }
  624. //else继续等待,直到任务完成
  625. }
  626. else
  627. {
  628. //直接降级
  629. m_carrier_status = E_READY;
  630. }
  631. break;
  632. }
  633. case E_ONE_LEVEL_OVER:
  634. {
  635. if ( mp_carrier_one_level_task.get() != NULL )
  636. {
  637. //检查任务状态,
  638. //在 E_ONE_LEVEL_WORK 里面, 已经 设为 TASK_OVER 了.
  639. //等待发送方的新指令, (发送方会把状态改为回收, 表示这个任务结束)
  640. if ( mp_carrier_one_level_task->get_task_statu() == TASK_WITHDRAW )
  641. {
  642. //这里会通知任务已经释放, 然后销毁任务单, 并降级
  643. mp_carrier_one_level_task->set_task_statu(TASK_FREE);
  644. mp_carrier_one_level_task.reset();
  645. m_carrier_status = E_READY;
  646. }
  647. //任务单重新创建, 调度创建的新的任务,那么回去继续工作
  648. else if ( mp_carrier_one_level_task->get_task_statu() == TASK_CREATED )
  649. {
  650. mp_carrier_one_level_task->set_task_statu(TASK_SIGNED);
  651. m_carrier_status = E_ONE_LEVEL_WORK;
  652. }
  653. //else //保持不动, 直到发送方给定新的任务,
  654. }
  655. else
  656. {
  657. //直接降级
  658. m_carrier_status = E_READY;
  659. }
  660. break;
  661. }
  662. case E_FAULT:
  663. {
  664. //更新通信
  665. update_device_communication();
  666. //所有任务报错, 并销毁任务.
  667. if ( mp_carrier_one_level_task.get() != NULL )
  668. {
  669. //添加错误码
  670. Error_manager t_error(CARRIER_STATUS_ERROR, MINOR_ERROR, "m_respons_status is error");
  671. mp_carrier_one_level_task->set_task_error_manager(t_error);
  672. end_task(mp_carrier_one_level_task);
  673. mp_carrier_one_level_task.reset();
  674. }
  675. if ( mp_carrier_two_level_task.get() != NULL )
  676. {
  677. //添加错误码
  678. Error_manager t_error(CARRIER_STATUS_ERROR, MINOR_ERROR, "m_respons_status is error");
  679. mp_carrier_two_level_task->set_task_error_manager(t_error);
  680. end_task(mp_carrier_two_level_task);
  681. mp_carrier_two_level_task.reset();
  682. }
  683. if ( mp_carrier_three_level_task.get() != NULL )
  684. {
  685. //添加错误码
  686. Error_manager t_error(CARRIER_STATUS_ERROR, MINOR_ERROR, "m_respons_status is error");
  687. mp_carrier_three_level_task->set_task_error_manager(t_error);
  688. end_task(mp_carrier_three_level_task);
  689. mp_carrier_three_level_task.reset();
  690. }
  691. break;
  692. }
  693. case E_DISCONNECT:
  694. {
  695. //更新通信
  696. update_device_communication();
  697. //所有任务报错, 并销毁任务.
  698. if ( mp_carrier_one_level_task.get() != NULL )
  699. {
  700. //添加错误码
  701. Error_manager t_error(CARRIER_STATUS_DISCONNECT, MINOR_ERROR, "m_respons_status is error");
  702. mp_carrier_one_level_task->set_task_error_manager(t_error);
  703. end_task(mp_carrier_one_level_task);
  704. mp_carrier_one_level_task.reset();
  705. }
  706. if ( mp_carrier_two_level_task.get() != NULL )
  707. {
  708. //添加错误码
  709. Error_manager t_error(CARRIER_STATUS_DISCONNECT, MINOR_ERROR, "m_respons_status is error");
  710. mp_carrier_two_level_task->set_task_error_manager(t_error);
  711. end_task(mp_carrier_two_level_task);
  712. mp_carrier_two_level_task.reset();
  713. }
  714. if ( mp_carrier_three_level_task.get() != NULL )
  715. {
  716. //添加错误码
  717. Error_manager t_error(CARRIER_STATUS_DISCONNECT, MINOR_ERROR, "m_respons_status is error");
  718. mp_carrier_three_level_task->set_task_error_manager(t_error);
  719. end_task(mp_carrier_three_level_task);
  720. mp_carrier_three_level_task.reset();
  721. }
  722. break;
  723. }
  724. case E_READY:
  725. {
  726. //更新通信
  727. update_device_communication();
  728. break;
  729. }
  730. default:
  731. {
  732. break;
  733. }
  734. }
  735. }
  736. }
  737. LOG(INFO) << " execute_thread_fun end "<< this;
  738. return;
  739. }
  740. //执行线程工作函数, 正在执行任务单.
  741. Error_manager Carrier_base::execute_thread_working(std::shared_ptr<Carrier_task> p_carrier_task, Carrier_status & carrier_status)
  742. {
  743. return Error_code::SUCCESS;
  744. }
  745. //执行线程工作函数, 已经完成任务单. 等待新的指令
  746. Error_manager Carrier_base::execute_thread_over(std::shared_ptr<Carrier_task> p_carrier_task, Carrier_status & carrier_status)
  747. {
  748. return Error_code::SUCCESS;
  749. }
  750. //更新设备底层通信数据
  751. Error_manager Carrier_base::update_device_communication()
  752. {
  753. return Error_code::SUCCESS;
  754. }
  755. //取消下发的指令
  756. Error_manager Carrier_base::cancel_command()
  757. {
  758. //以后再写 need programe
  759. //目前调度和plc的通信指令做的很简单,没有暂停和急停 复位等操作.
  760. //这里先空着,以后再写.
  761. //调度模块单方面销毁任务, 不管底层plc的执行情况, 也不去告知plc任务取消.
  762. return Error_code::SUCCESS;
  763. }
  764. */