carrier.cpp 20 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404
  1. //
  2. // Created by huli on 2021/3/12.
  3. //
  4. #include "carrier.h"
  5. Carrier::Carrier()
  6. {
  7. m_request_x = 0; //搬运器坐标x轴,
  8. m_request_y = 0; //搬运器坐标y轴,
  9. m_request_z = 0; //搬运器坐标z轴,
  10. m_request_y1 = 0; //搬运器坐标y1轴, 搬运器抓车杆纵向移动(前轮抓杆)
  11. m_request_y2 = 0; //搬运器坐标y2轴, 搬运器抓车杆纵向移动(后轮抓杆)
  12. m_request_clamp_motion = E_CLAMP_NO_ACTION; //搬运器夹车杆. 0=无动作,1=夹紧,2=松开
  13. m_request_joint_motion_x = E_JOINT_NO_ACTION; //电梯与X轴的横向轨道对接,0=无动作,1=进行对接, 2=松开对接
  14. m_request_joint_motion_y = E_JOINT_NO_ACTION; //小跑车与Y轴的横向轨道对接,0=无动作,1=进行对接, 2=松开对接
  15. m_request_space_id = 0; //搬运器空间位置的id.
  16. m_request_floor_id = 0; //搬运器楼层位置的id.
  17. m_request_wheelbase = 0; //搬运器抓车杆前后轮距.
  18. m_respons_status = RESPONS_WORKING; //指令完成状态, 搬运器答复指令, 返回任务完成的情况
  19. m_respons_x = 0; //搬运器坐标x轴,
  20. m_respons_y = 0; //搬运器坐标y轴,
  21. m_respons_z = 0; //搬运器坐标z轴,
  22. m_respons_y1 = 0; //搬运器坐标y1轴, 搬运器抓车杆纵向移动(前轮抓杆)
  23. m_respons_y2 = 0; //搬运器坐标y2轴, 搬运器抓车杆纵向移动(后轮抓杆)
  24. m_respons_clamp_motion = E_CLAMP_NO_ACTION; //搬运器夹车杆. 0=无动作,1=夹紧,2=松开
  25. m_respons_joint_motion_x = E_JOINT_NO_ACTION; //电梯与X轴的横向轨道对接,0=无动作,1=进行对接, 2=松开对接
  26. m_respons_joint_motion_y = E_JOINT_NO_ACTION; //小跑车与Y轴的横向轨道对接,0=无动作,1=进行对接, 2=松开对接
  27. m_respons_space_id = 0; //搬运器空间位置的id.
  28. m_respons_floor_id = 0; //搬运器楼层位置的id.
  29. m_respons_wheelbase = 0; //搬运器抓车杆前后轮距.
  30. m_status_updata_time = std::chrono::system_clock::now();
  31. m_last_heartbeat = 0; //上一次的心跳
  32. m_actual_device_status = HARDWARE_DEVICE_UNKNOWN; //搬运器的硬件设备状态
  33. m_actual_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_small_sports_car_motion = E_SMALL_SPORTS_NO_ACTION;
  42. m_actual_joint_motion_x1 = E_JOINT_NO_ACTION; //电梯与X轴的横向轨道对接,0=无动作,1=进行对接, 2=松开对接
  43. m_actual_joint_motion_x2 = E_JOINT_NO_ACTION; //电梯与X轴的横向轨道对接,0=无动作,1=进行对接, 2=松开对接
  44. memset(m_actual_error_code, 0, 50); //搬运器设备的报警信息位
  45. memset(m_actual_warning_code, 0, 50); //升降机设备的报警信息位
  46. m_actual_coordinates_id = 0;
  47. }
  48. Carrier::~Carrier()
  49. {
  50. }
  51. //检查任务类型, 子类必须重载, 用来检查输入的任务是否为子类所需的.
  52. Error_manager Carrier::check_task_type(std::shared_ptr<Task_Base> p_task)
  53. {
  54. //检查任务类型,
  55. if (p_task->get_task_type() != Task_Base::Task_type::CARRIER_TASK)
  56. {
  57. return Error_manager(Error_code::CARRIER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
  58. "Carrier::check_task_type get_task_type() != CARRIER_TASK ");
  59. }
  60. return Error_code::SUCCESS;
  61. }
  62. //获取硬件设备的状态, 必须子类继承
  63. Carrier::Hardware_device_status Carrier::get_actual_device_status()
  64. {
  65. return m_actual_device_status;
  66. }
  67. //把任务单写入到内存中, 子类必须重载
  68. Error_manager Carrier::write_task_to_memory(std::shared_ptr<Task_Base> p_task)
  69. {
  70. //检查任务类型,
  71. if (p_task->get_task_type() != Task_Base::Task_type::CARRIER_TASK)
  72. {
  73. return Error_manager(Error_code::CARRIER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
  74. "Carrier::check_task_type get_task_type() != CARRIER_TASK ");
  75. }
  76. else
  77. {
  78. std::unique_lock<std::mutex> t_lock(m_lock);
  79. Carrier_task* tp_carrier_task = (Carrier_task*)p_task.get();
  80. std::unique_lock<std::mutex> t_lock1(tp_carrier_task->m_lock);
  81. m_request_key = tp_carrier_task->m_request_key;
  82. m_request_x = tp_carrier_task->m_request_x;
  83. m_request_y = tp_carrier_task->m_request_y;
  84. m_request_z = tp_carrier_task->m_request_z;
  85. m_request_y1 = tp_carrier_task->m_request_y1;
  86. m_request_y2 = tp_carrier_task->m_request_y2;
  87. m_request_clamp_motion = (Dispatch_device_base::Clamp_motion)tp_carrier_task->m_request_clamp_motion;
  88. m_request_joint_motion_x = (Dispatch_device_base::Joint_motion)tp_carrier_task->m_request_joint_motion_x;
  89. m_request_joint_motion_y = (Dispatch_device_base::Joint_motion)tp_carrier_task->m_request_joint_motion_y;
  90. m_request_space_id = tp_carrier_task->m_request_space_id;
  91. m_request_floor_id = tp_carrier_task->m_request_floor_id;
  92. m_request_wheelbase = tp_carrier_task->m_request_wheelbase;
  93. return Error_code::SUCCESS;
  94. }
  95. return Error_code::SUCCESS;
  96. }
  97. //更新设备底层通信数据, 子类必须重载
  98. Error_manager Carrier::update_device_communication()
  99. {
  100. std::unique_lock<std::mutex> t_lock1(Dispatch_communication::get_instance_references().m_data_lock);
  101. std::unique_lock<std::mutex> t_lock(m_lock);
  102. //请求消息, 调度->plc
  103. Dispatch_communication::Carrier_request_from_dispatch_to_plc_for_data * tp_carrier_request_from_dispatch_to_plc_for_data =
  104. & Dispatch_communication::get_instance_references().m_carrier_request_from_dispatch_to_plc_for_data[m_device_id];
  105. Dispatch_communication::Carrier_request_from_dispatch_to_plc_for_key * tp_carrier_request_from_dispatch_to_plc_for_key =
  106. & Dispatch_communication::get_instance_references().m_carrier_request_from_dispatch_to_plc_for_key[m_device_id];
  107. memset(tp_carrier_request_from_dispatch_to_plc_for_key->m_request_key, 0, 50);
  108. int t_size = m_request_key.size()<=50 ? m_request_key.size() : 50 ;
  109. memcpy(tp_carrier_request_from_dispatch_to_plc_for_key->m_request_key, m_request_key.c_str(), t_size);
  110. tp_carrier_request_from_dispatch_to_plc_for_data->m_request_x = m_request_x;
  111. tp_carrier_request_from_dispatch_to_plc_for_data->m_request_y = m_request_y;
  112. tp_carrier_request_from_dispatch_to_plc_for_data->m_request_z = m_request_z;
  113. tp_carrier_request_from_dispatch_to_plc_for_data->m_request_y1 = m_request_y1;
  114. tp_carrier_request_from_dispatch_to_plc_for_data->m_request_y2 = m_request_y2;
  115. tp_carrier_request_from_dispatch_to_plc_for_data->m_request_clamp_motion = m_request_clamp_motion;
  116. tp_carrier_request_from_dispatch_to_plc_for_data->m_request_joint_motion_x = m_request_joint_motion_x;
  117. tp_carrier_request_from_dispatch_to_plc_for_data->m_request_joint_motion_y = m_request_joint_motion_y;
  118. tp_carrier_request_from_dispatch_to_plc_for_data->m_request_space_id = m_request_space_id;
  119. tp_carrier_request_from_dispatch_to_plc_for_data->m_request_floor_id = m_request_floor_id;
  120. tp_carrier_request_from_dispatch_to_plc_for_data->m_request_wheelbase = m_request_wheelbase;
  121. //答复消息, plc->调度
  122. Dispatch_communication::Carrier_response_from_plc_to_dispatch * tp_carrier_response_from_plc_to_dispatch =
  123. & Dispatch_communication::get_instance_references().m_carrier_response_from_plc_to_dispatch[m_device_id];
  124. m_respons_key = (char*) tp_carrier_response_from_plc_to_dispatch->m_respons_key;
  125. m_respons_status = (Dispatch_device_base::Respons_status)tp_carrier_response_from_plc_to_dispatch->m_respons_status;
  126. m_respons_x = tp_carrier_response_from_plc_to_dispatch->m_respons_x;
  127. m_respons_y = tp_carrier_response_from_plc_to_dispatch->m_respons_y;
  128. m_respons_z = tp_carrier_response_from_plc_to_dispatch->m_respons_z;
  129. m_respons_y1 = tp_carrier_response_from_plc_to_dispatch->m_respons_y1;
  130. m_respons_y2 = tp_carrier_response_from_plc_to_dispatch->m_respons_y2;
  131. m_respons_clamp_motion = (Dispatch_device_base::Clamp_motion)tp_carrier_response_from_plc_to_dispatch->m_respons_clamp_motion;
  132. m_respons_joint_motion_x = (Dispatch_device_base::Joint_motion)tp_carrier_response_from_plc_to_dispatch->m_respons_joint_motion_x;
  133. m_respons_joint_motion_y = (Dispatch_device_base::Joint_motion)tp_carrier_response_from_plc_to_dispatch->m_respons_joint_motion_y;
  134. m_respons_space_id = tp_carrier_response_from_plc_to_dispatch->m_respons_space_id;
  135. m_respons_floor_id = tp_carrier_response_from_plc_to_dispatch->m_respons_floor_id;
  136. m_respons_wheelbase = tp_carrier_response_from_plc_to_dispatch->m_respons_wheelbase;
  137. //状态消息, plc->调度
  138. Dispatch_communication::Carrier_status_from_plc_to_dispatch *tp_carrier_status_from_plc_to_dispatch =
  139. & Dispatch_communication::get_instance_references().m_carrier_status_from_plc_to_dispatch[m_device_id];
  140. //设备异常 //注注注注注注注注意了, ==的优先级比&要高.
  141. if ( (tp_carrier_status_from_plc_to_dispatch->m_safe_status & 0x02) == 0 )
  142. {
  143. m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_EMERGENCY_STOP;
  144. m_dispatch_device_status = Dispatch_device_base::DISPATCH_DEVICE_FAULT;
  145. }
  146. else if ( (tp_carrier_status_from_plc_to_dispatch->m_safe_status & 0x01) == 0 )
  147. {
  148. m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_FAULT;
  149. m_dispatch_device_status = Dispatch_device_base::DISPATCH_DEVICE_FAULT;
  150. }
  151. else if ( (tp_carrier_status_from_plc_to_dispatch->m_safe_status & 0x08) == 0 )
  152. {
  153. m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_COLLISION;
  154. m_dispatch_device_status = Dispatch_device_base::DISPATCH_DEVICE_FAULT;
  155. }
  156. //正常状态
  157. else
  158. {
  159. if (tp_carrier_status_from_plc_to_dispatch->m_work_status == 1)
  160. {
  161. m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_WORKING;
  162. }
  163. else if(tp_carrier_status_from_plc_to_dispatch->m_work_status == 2)
  164. {
  165. m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_READY;
  166. }
  167. else if(tp_carrier_status_from_plc_to_dispatch->m_work_status == 0)
  168. {
  169. m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_UNKNOWN;
  170. }
  171. //故障恢复之后 E_FAULT ->> E_THREE_LEVEL_WORK
  172. if ( m_dispatch_device_status == Dispatch_device_base::DISPATCH_DEVICE_FAULT )
  173. {
  174. m_dispatch_device_status = Dispatch_device_base::DISPATCH_DEVICE_THREE_LEVEL_WORK;
  175. }
  176. //else 流程状态维持不变
  177. }
  178. m_actual_load_status = (Dispatch_device_base::Load_status)tp_carrier_status_from_plc_to_dispatch->m_actual_load_status;
  179. m_actual_x = tp_carrier_status_from_plc_to_dispatch->m_actual_x;
  180. m_actual_y = tp_carrier_status_from_plc_to_dispatch->m_actual_y;
  181. if ( m_device_id == 2 )
  182. {
  183. m_actual_z = Dispatch_coordinates::get_instance_references().m_carrier_3th_floor_z;
  184. }
  185. else
  186. {
  187. m_actual_z = tp_carrier_status_from_plc_to_dispatch->m_actual_z;
  188. }
  189. m_actual_y1 = tp_carrier_status_from_plc_to_dispatch->m_actual_y1;
  190. m_actual_y2 = tp_carrier_status_from_plc_to_dispatch->m_actual_y2;
  191. m_actual_clamp_motion1 = (Dispatch_device_base::Clamp_motion)tp_carrier_status_from_plc_to_dispatch->m_actual_clamp_motion1;
  192. m_actual_clamp_motion2 = (Dispatch_device_base::Clamp_motion)tp_carrier_status_from_plc_to_dispatch->m_actual_clamp_motion2;
  193. m_actual_small_sports_car_motion = (Dispatch_device_base::Small_sports_car_motion)tp_carrier_status_from_plc_to_dispatch->m_actual_small_sports_car_motion;
  194. m_actual_joint_motion_x1 = (Dispatch_device_base::Joint_motion)tp_carrier_status_from_plc_to_dispatch->m_actual_joint_motion_x1;
  195. m_actual_joint_motion_x2 = (Dispatch_device_base::Joint_motion)tp_carrier_status_from_plc_to_dispatch->m_actual_joint_motion_x2;
  196. memcpy(m_actual_error_code, tp_carrier_status_from_plc_to_dispatch->m_actual_error_code, 50);
  197. memcpy(m_actual_warning_code, tp_carrier_status_from_plc_to_dispatch->m_actual_warning_code, 50);
  198. m_actual_error_description = (char*)(tp_carrier_status_from_plc_to_dispatch->m_actual_error_description-2);
  199. update_actual_coordinates_id();
  200. //通过心跳帧来判断通信是否正常
  201. if ( m_last_heartbeat != tp_carrier_status_from_plc_to_dispatch->m_heartbeat )
  202. {
  203. m_last_heartbeat = tp_carrier_status_from_plc_to_dispatch->m_heartbeat;
  204. m_status_updata_time = std::chrono::system_clock::now();
  205. // std::cout << " 111111111111111111111111111 = " << 123 << std::endl;
  206. // std::cout << " tp_carrier_request_from_dispatch_to_plc_for_key->m_request_key = " << tp_carrier_request_from_dispatch_to_plc_for_key->m_request_key << std::endl;
  207. // std::cout << " tp_carrier_response_from_plc_to_dispatch->m_respons_key = " << tp_carrier_response_from_plc_to_dispatch->m_respons_key << std::endl;
  208. // int a = tp_carrier_status_from_plc_to_dispatch->m_work_status;
  209. // std::cout << " tp_carrier_status_from_plc_to_dispatch->m_work_status = " << a << std::endl;
  210. // std::cout << " m_request_key = " << m_request_key << std::endl;
  211. // std::cout << " m_respons_key = " << m_respons_key << std::endl;
  212. // std::cout << " m_actual_device_status = " << m_actual_device_status << std::endl;
  213. // std::cout << " 111111111111111111111111111 = " << 456 << std::endl;
  214. //重连之后,搬运器状态 E_DISCONNECT ->> E_THREE_LEVEL_WORK
  215. if ( m_dispatch_device_status == Dispatch_device_base::DISPATCH_DEVICE_DISCONNECT )
  216. {
  217. m_dispatch_device_status = Dispatch_device_base::DISPATCH_DEVICE_THREE_LEVEL_WORK;
  218. }
  219. }
  220. else if(std::chrono::system_clock::now() - m_status_updata_time > std::chrono::milliseconds(COMMUNICATION_OVER_TIME_MS))
  221. {
  222. m_dispatch_device_status = Dispatch_device_base::DISPATCH_DEVICE_DISCONNECT;
  223. }
  224. //else 继续等待,直到消息刷新或者超时.
  225. return Error_code::SUCCESS;
  226. }
  227. //从内存中读数据到任务单, 子类必须重载
  228. Error_manager Carrier::check_and_read_memory_to_task(std::shared_ptr<Task_Base> p_task)
  229. {
  230. // std::cout << " huli test :::: " << " ((((((((((((((((((((((((((333333333333333333 = " << 333 << std::endl;
  231. // std::cout << " huli test :::: " << " m_request_key = " << m_request_key << std::endl;
  232. // std::cout << " huli test :::: " << " m_respons_key = " << m_respons_key << std::endl;
  233. // std::cout << " huli test :::: " << " m_respons_status = " << m_respons_status << std::endl;
  234. // std::cout << " huli test :::: " << " )))))))))))))))))))))))))))))))4444444444444 = " << 444 << std::endl;
  235. Dispatch_communication::get_instance_references().communication_start();
  236. //检查任务类型,
  237. if (p_task->get_task_type() != Task_Base::Task_type::CARRIER_TASK)
  238. {
  239. return Error_manager(Error_code::CARRIER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
  240. "Carrier::check_task_type get_task_type() != CARRIER_TASK ");
  241. }
  242. else
  243. {
  244. std::unique_lock<std::mutex> t_lock(m_lock);
  245. if ( m_respons_key == m_request_key && m_respons_status != RESPONS_WORKING )
  246. {
  247. Carrier_task* tp_carrier_task = (Carrier_task*)p_task.get();
  248. std::unique_lock<std::mutex> t_lock1(tp_carrier_task->m_lock);
  249. tp_carrier_task->m_respons_key = m_respons_key;
  250. tp_carrier_task->m_respons_status = (Carrier_task::Respons_status)m_respons_status;
  251. tp_carrier_task->m_respons_x = m_respons_x;
  252. tp_carrier_task->m_respons_y = m_respons_y;
  253. tp_carrier_task->m_respons_z = m_respons_z;
  254. tp_carrier_task->m_respons_y1 = m_respons_y1;
  255. tp_carrier_task->m_respons_y2 = m_respons_y2;
  256. tp_carrier_task->m_respons_clamp_motion = (Carrier_task::Clamp_motion)m_respons_clamp_motion;
  257. tp_carrier_task->m_respons_joint_motion_x = (Carrier_task::Joint_motion)m_respons_joint_motion_x;
  258. tp_carrier_task->m_respons_joint_motion_y = (Carrier_task::Joint_motion)m_respons_joint_motion_y;
  259. tp_carrier_task->m_respons_space_id = m_respons_space_id;
  260. tp_carrier_task->m_respons_floor_id = m_respons_floor_id;
  261. tp_carrier_task->m_respons_wheelbase = m_respons_wheelbase;
  262. //如果故障,则添加错误码
  263. if ( m_respons_status == RESPONS_MINOR_ERROR || m_respons_status == RESPONS_CRITICAL_ERROR )
  264. {
  265. //添加错误码
  266. Error_manager t_error(CARRIER_RESPONS_ERROR, MINOR_ERROR, "m_respons_status is error");
  267. tp_carrier_task->set_task_error_manager(t_error);
  268. }
  269. std::cout << " huli test :::: " << " {{{{{{{{{{{{{{{{{{{{{ 33333333333333333333333333333333333333333 }}}}}}}}}}}}}}}}}}}}}}}}}}= " << 333 << std::endl;
  270. std::cout << " huli test :::: " << " m_request_key = " << m_request_key << std::endl;
  271. std::cout << " huli test :::: " << " m_request_x = " << m_request_x << std::endl;
  272. std::cout << " huli test :::: " << " m_request_y = " << m_request_y << std::endl;
  273. std::cout << " huli test :::: " << " m_request_z = " << m_request_z << std::endl;
  274. std::cout << " huli test :::: " << " m_request_clamp_motion = " << m_request_clamp_motion << std::endl;
  275. std::cout << " huli test :::: " << " m_request_wheelbase = " << m_request_wheelbase << std::endl;
  276. std::cout << " huli test :::: " << " m_request_y1 = " << m_request_y1 << std::endl;
  277. std::cout << " huli test :::: " << " m_request_y2 = " << m_request_y2 << std::endl;
  278. std::cout << " m_request_joint_motion_x = " << m_request_joint_motion_x << std::endl;
  279. std::cout << " huli test :::: " << " m_respons_key = " << m_respons_key << std::endl;
  280. std::cout << " huli test :::: " << " m_respons_status = " << m_respons_status << std::endl;
  281. std::cout << " huli test :::: " << " m_respons_x = " << m_respons_x << std::endl;
  282. std::cout << " huli test :::: " << " m_respons_y = " << m_respons_y << std::endl;
  283. std::cout << " huli test :::: " << " m_respons_z = " << m_respons_z << std::endl;
  284. std::cout << " huli test :::: " << " m_respons_clamp_motion = " << m_respons_clamp_motion << std::endl;
  285. std::cout << " huli test :::: " << " m_respons_wheelbase = " << m_respons_wheelbase << std::endl;
  286. std::cout << " huli test :::: " << " m_respons_y1 = " << m_respons_y1 << std::endl;
  287. std::cout << " huli test :::: " << " m_respons_y2 = " << m_respons_y2 << std::endl;
  288. std::cout << " m_respons_joint_motion_x = " << m_respons_joint_motion_x << std::endl;
  289. std::cout << " m_actual_device_status = " << m_actual_device_status << std::endl;
  290. std::cout << " huli test :::: " << " m_actual_x = " << m_actual_x << std::endl;
  291. std::cout << " huli test :::: " << " m_actual_y = " << m_actual_y << std::endl;
  292. std::cout << " huli test :::: " << " m_actual_z = " << m_actual_z << std::endl;
  293. std::cout << " huli test :::: " << " m_actual_y1 = " << m_actual_y1 << std::endl;
  294. std::cout << " huli test :::: " << " m_actual_y2 = " << m_actual_y2 << std::endl;
  295. std::cout << " huli test :::: " << " m_actual_clamp_motion1 = " << m_actual_clamp_motion1 << std::endl;
  296. std::cout << " huli test :::: " << " {{{{{{{{{{{{{{{{{{{{{{{{{{{{ 44444444444444444444444444444444444444444 }}}}}}}}}}}}}}}}}}}}}}}}= " << 444 << std::endl;
  297. return Error_code::SUCCESS;
  298. }
  299. //返回没有收到数据
  300. else
  301. {
  302. return Error_code::NODATA;
  303. }
  304. }
  305. return Error_code::SUCCESS;
  306. }
  307. //取消下发的指令
  308. Error_manager Carrier::cancel_command()
  309. {
  310. //以后再写 need programe
  311. //目前调度和plc的通信指令做的很简单,没有暂停和急停 复位等操作.
  312. //这里先空着,以后再写.
  313. //调度模块单方面销毁任务, 不管底层plc的执行情况, 也不去告知plc任务取消.
  314. return Error_code::SUCCESS;
  315. }
  316. //更新真实 空间位置的id.
  317. Error_manager Carrier::update_actual_coordinates_id()
  318. {
  319. Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();
  320. if ( get_device_id() == 2 )
  321. {
  322. for (auto iter = tp_dispatch_coordinates->m_carrier_coordinates.begin(); iter != tp_dispatch_coordinates->m_carrier_coordinates.end(); ++iter)
  323. {
  324. if ( Common_data::approximate_difference(m_actual_x, iter->second.x, 100) &&
  325. Common_data::approximate_difference(m_actual_y, iter->second.y, 500) &&
  326. Common_data::approximate_difference(tp_dispatch_coordinates->m_carrier_3th_floor_z, iter->second.z, 200) )
  327. {
  328. m_actual_coordinates_id = iter->first;
  329. }
  330. }
  331. }
  332. else
  333. {
  334. for (auto iter = tp_dispatch_coordinates->m_carrier_coordinates.begin(); iter != tp_dispatch_coordinates->m_carrier_coordinates.end(); ++iter)
  335. {
  336. if ( Common_data::approximate_difference(m_actual_x, iter->second.x, 100) &&
  337. Common_data::approximate_difference(m_actual_y, iter->second.y, 500) &&
  338. Common_data::approximate_difference(m_actual_z, iter->second.z, 200) )
  339. {
  340. m_actual_coordinates_id = iter->first;
  341. }
  342. }
  343. }
  344. return Error_code::SUCCESS;
  345. }