// // Created by huli on 2021/3/12. // #include "carrier.h" #include "dispatch_manager.h" Carrier::Carrier() { m_request_x = 0; //搬运器坐标x轴, m_request_y = 0; //搬运器坐标y轴, m_request_z = 0; //搬运器坐标z轴, m_request_y1 = 0; //搬运器坐标y1轴, 搬运器抓车杆纵向移动(前轮抓杆) m_request_y2 = 0; //搬运器坐标y2轴, 搬运器抓车杆纵向移动(后轮抓杆) m_request_clamp_motion = E_CLAMP_NO_ACTION; //搬运器夹车杆. 0=无动作，1=夹紧，2=松开 m_request_joint_motion_x = E_JOINT_NO_ACTION; //电梯与X轴的横向轨道对接,0=无动作，1=进行对接, 2=松开对接 m_request_joint_motion_y = E_JOINT_NO_ACTION; //小跑车与Y轴的横向轨道对接,0=无动作，1=进行对接, 2=松开对接 m_request_space_id = 0; //搬运器空间位置的id. m_request_floor_id = 0; //搬运器楼层位置的id. m_request_wheelbase = 0; //搬运器抓车杆前后轮距. m_respons_status = RESPONS_WORKING; //指令完成状态, 搬运器答复指令, 返回任务完成的情况 m_respons_x = 0; //搬运器坐标x轴, m_respons_y = 0; //搬运器坐标y轴, m_respons_z = 0; //搬运器坐标z轴, m_respons_y1 = 0; //搬运器坐标y1轴, 搬运器抓车杆纵向移动(前轮抓杆) m_respons_y2 = 0; //搬运器坐标y2轴, 搬运器抓车杆纵向移动(后轮抓杆) m_respons_clamp_motion = E_CLAMP_NO_ACTION; //搬运器夹车杆. 0=无动作，1=夹紧，2=松开 m_respons_joint_motion_x = E_JOINT_NO_ACTION; //电梯与X轴的横向轨道对接,0=无动作，1=进行对接, 2=松开对接 m_respons_joint_motion_y = E_JOINT_NO_ACTION; //小跑车与Y轴的横向轨道对接,0=无动作，1=进行对接, 2=松开对接 m_respons_space_id = 0; //搬运器空间位置的id. m_respons_floor_id = 0; //搬运器楼层位置的id. m_respons_wheelbase = 0; //搬运器抓车杆前后轮距. m_status_updata_time = std::chrono::system_clock::now(); m_last_heartbeat = 0; //上一次的心跳 m_actual_device_status = HARDWARE_DEVICE_UNKNOWN; //搬运器的硬件设备状态 m_actual_load_status = LOAD_UNKNOWN; //搬运器的负载状态, 小跑车上面是否有车. m_actual_x = 0; //搬运器坐标x轴, m_actual_y = 0; //搬运器坐标y轴, m_actual_z = 0; //搬运器坐标z轴, m_actual_y1 = 0; //搬运器坐标y1轴, 搬运器抓车杆纵向移动(前轮抓杆) m_actual_y2 = 0; //搬运器坐标y2轴, 搬运器抓车杆纵向移动(后轮抓杆) m_actual_clamp_motion1 = E_CLAMP_NO_ACTION; //小跑车夹车杆. 0=无动作，1=夹紧，2=松开 m_actual_clamp_motion2 = E_CLAMP_NO_ACTION; //小跑车夹车杆. 0=无动作，1=夹紧，2=松开 m_actual_small_sports_car_motion = E_SMALL_SPORTS_NO_ACTION; m_actual_joint_motion_x1 = E_JOINT_NO_ACTION; //电梯与X轴的横向轨道对接,0=无动作，1=进行对接, 2=松开对接 m_actual_joint_motion_x2 = E_JOINT_NO_ACTION; //电梯与X轴的横向轨道对接,0=无动作，1=进行对接, 2=松开对接 memset(m_actual_error_code, 0, 50); //搬运器设备的报警信息位 memset(m_actual_warning_code, 0, 50); //升降机设备的报警信息位 m_actual_coordinates_id = 0; m_actual_coordinates_rows = 0; m_actual_coordinates_columns = 0; m_redirect_flag = false; m_catcher_run_flag = false; m_redirect_pickup_parkspace_id = 0; m_redirect_pickup_car_type = Common_data::Car_type::UNKNOW_CAR_TYPE; m_redirect_store_parkspace_id = 0; m_redirect_store_car_type = Common_data::Car_type::UNKNOW_CAR_TYPE; } Carrier::~Carrier() { } //检查任务类型, 子类必须重载, 用来检查输入的任务是否为子类所需的. Error_manager Carrier::check_task_type(std::shared_ptr p_task) { //检查任务类型， if (p_task->get_task_type() != Task_Base::Task_type::CARRIER_TASK) { return Error_manager(Error_code::CARRIER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR, "Carrier::check_task_type get_task_type() != CARRIER_TASK "); } return Error_code::SUCCESS; } //获取硬件设备的状态, 必须子类继承 Carrier::Hardware_device_status Carrier::get_actual_device_status() { return m_actual_device_status; } std::string Carrier::get_current_command_key() { return (m_request_key + " +++ " + m_respons_key); } //把任务单写入到内存中, 子类必须重载 Error_manager Carrier::write_task_to_memory(std::shared_ptr p_task) { //检查任务类型， if (p_task->get_task_type() != Task_Base::Task_type::CARRIER_TASK) { return Error_manager(Error_code::CARRIER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR, "Carrier::check_task_type get_task_type() != CARRIER_TASK "); } else { Carrier_task* tp_carrier_task = (Carrier_task*)p_task.get(); std::unique_lock t_lock1(tp_carrier_task->m_lock); m_request_key = tp_carrier_task->m_request_key; m_request_x = tp_carrier_task->m_request_x; m_request_y = tp_carrier_task->m_request_y; m_request_z = tp_carrier_task->m_request_z; m_request_y1 = tp_carrier_task->m_request_y1; m_request_y2 = tp_carrier_task->m_request_y2; m_request_clamp_motion = (Dispatch_device_base::Clamp_motion)tp_carrier_task->m_request_clamp_motion; m_request_joint_motion_x = (Dispatch_device_base::Joint_motion)tp_carrier_task->m_request_joint_motion_x; m_request_joint_motion_y = (Dispatch_device_base::Joint_motion)tp_carrier_task->m_request_joint_motion_y; m_request_space_id = tp_carrier_task->m_request_space_id; m_request_floor_id = tp_carrier_task->m_request_floor_id; m_request_wheelbase = tp_carrier_task->m_request_wheelbase; return Error_code::SUCCESS; } return Error_code::SUCCESS; } //更新设备底层通信数据, 子类必须重载 Error_manager Carrier::update_device_communication() { std::unique_lock t_lock1(Dispatch_communication::get_instance_references().m_data_lock); //请求消息, 调度->plc Dispatch_communication::Carrier_request_from_dispatch_to_plc_for_data * tp_carrier_request_from_dispatch_to_plc_for_data = & Dispatch_communication::get_instance_references().m_carrier_request_from_dispatch_to_plc_for_data[m_device_id]; Dispatch_communication::Carrier_request_from_dispatch_to_plc_for_key * tp_carrier_request_from_dispatch_to_plc_for_key = & Dispatch_communication::get_instance_references().m_carrier_request_from_dispatch_to_plc_for_key[m_device_id]; memset(tp_carrier_request_from_dispatch_to_plc_for_key->m_request_key, 0, 50); int t_size = m_request_key.size()<=50 ? m_request_key.size() : 50 ; memcpy(tp_carrier_request_from_dispatch_to_plc_for_key->m_request_key, m_request_key.c_str(), t_size); tp_carrier_request_from_dispatch_to_plc_for_data->m_request_x = m_request_x; tp_carrier_request_from_dispatch_to_plc_for_data->m_request_y = m_request_y; tp_carrier_request_from_dispatch_to_plc_for_data->m_request_z = m_request_z; tp_carrier_request_from_dispatch_to_plc_for_data->m_request_y1 = m_request_y1; tp_carrier_request_from_dispatch_to_plc_for_data->m_request_y2 = m_request_y2; tp_carrier_request_from_dispatch_to_plc_for_data->m_request_clamp_motion = m_request_clamp_motion; tp_carrier_request_from_dispatch_to_plc_for_data->m_request_joint_motion_x = m_request_joint_motion_x; tp_carrier_request_from_dispatch_to_plc_for_data->m_request_joint_motion_y = m_request_joint_motion_y; tp_carrier_request_from_dispatch_to_plc_for_data->m_request_space_id = m_request_space_id; tp_carrier_request_from_dispatch_to_plc_for_data->m_request_floor_id = m_request_floor_id; tp_carrier_request_from_dispatch_to_plc_for_data->m_request_wheelbase = m_request_wheelbase; //答复消息, plc->调度 Dispatch_communication::Carrier_response_from_plc_to_dispatch * tp_carrier_response_from_plc_to_dispatch = & Dispatch_communication::get_instance_references().m_carrier_response_from_plc_to_dispatch[m_device_id]; m_respons_key = (char*) tp_carrier_response_from_plc_to_dispatch->m_respons_key; m_respons_status = (Dispatch_device_base::Respons_status)tp_carrier_response_from_plc_to_dispatch->m_respons_status; m_respons_x = tp_carrier_response_from_plc_to_dispatch->m_respons_x; m_respons_y = tp_carrier_response_from_plc_to_dispatch->m_respons_y; m_respons_z = tp_carrier_response_from_plc_to_dispatch->m_respons_z; m_respons_y1 = tp_carrier_response_from_plc_to_dispatch->m_respons_y1; m_respons_y2 = tp_carrier_response_from_plc_to_dispatch->m_respons_y2; m_respons_clamp_motion = (Dispatch_device_base::Clamp_motion)tp_carrier_response_from_plc_to_dispatch->m_respons_clamp_motion; m_respons_joint_motion_x = (Dispatch_device_base::Joint_motion)tp_carrier_response_from_plc_to_dispatch->m_respons_joint_motion_x; m_respons_joint_motion_y = (Dispatch_device_base::Joint_motion)tp_carrier_response_from_plc_to_dispatch->m_respons_joint_motion_y; m_respons_space_id = tp_carrier_response_from_plc_to_dispatch->m_respons_space_id; m_respons_floor_id = tp_carrier_response_from_plc_to_dispatch->m_respons_floor_id; m_respons_wheelbase = tp_carrier_response_from_plc_to_dispatch->m_respons_wheelbase; //状态消息, plc->调度 Dispatch_communication::Carrier_status_from_plc_to_dispatch *tp_carrier_status_from_plc_to_dispatch = & Dispatch_communication::get_instance_references().m_carrier_status_from_plc_to_dispatch[m_device_id]; //设备异常 //注注注注注注注注意了, ==的优先级比&要高. if ( (tp_carrier_status_from_plc_to_dispatch->m_safe_status & 0x02) == 0 ) { m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_EMERGENCY_STOP; m_dispatch_device_status = Dispatch_device_base::DISPATCH_DEVICE_FAULT; } else if ( (tp_carrier_status_from_plc_to_dispatch->m_safe_status & 0x01) == 0 ) { m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_FAULT; m_dispatch_device_status = Dispatch_device_base::DISPATCH_DEVICE_FAULT; } else if ( (tp_carrier_status_from_plc_to_dispatch->m_safe_status & 0x08) == 0 ) { m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_COLLISION; m_dispatch_device_status = Dispatch_device_base::DISPATCH_DEVICE_FAULT; } //正常状态 else { if (tp_carrier_status_from_plc_to_dispatch->m_work_status == 1) { m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_WORKING; } else if(tp_carrier_status_from_plc_to_dispatch->m_work_status == 2) { m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_READY; } else if(tp_carrier_status_from_plc_to_dispatch->m_work_status == 0) { m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_UNKNOWN; } //故障恢复之后 E_FAULT ->> E_THREE_LEVEL_WORK if ( m_dispatch_device_status == Dispatch_device_base::DISPATCH_DEVICE_FAULT ) { m_dispatch_device_status = Dispatch_device_base::DISPATCH_DEVICE_THREE_LEVEL_WORK; } //else 流程状态维持不变 } m_actual_load_status = (Dispatch_device_base::Load_status)tp_carrier_status_from_plc_to_dispatch->m_actual_load_status; m_actual_x = tp_carrier_status_from_plc_to_dispatch->m_actual_x; m_actual_y = tp_carrier_status_from_plc_to_dispatch->m_actual_y; //plc没有 给3楼的中跑车写入z轴, 所以手动写入z轴, 方便后面判断坐标 if ( m_device_id == 2 && m_actual_x >1) { m_actual_z = Dispatch_coordinates::get_instance_references().m_carrier_3th_floor_z; } else { m_actual_z = tp_carrier_status_from_plc_to_dispatch->m_actual_z; } m_actual_y1 = tp_carrier_status_from_plc_to_dispatch->m_actual_y1; m_actual_y2 = tp_carrier_status_from_plc_to_dispatch->m_actual_y2; m_actual_clamp_motion1 = (Dispatch_device_base::Clamp_motion)tp_carrier_status_from_plc_to_dispatch->m_actual_clamp_motion1; m_actual_clamp_motion2 = (Dispatch_device_base::Clamp_motion)tp_carrier_status_from_plc_to_dispatch->m_actual_clamp_motion2; 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; m_actual_joint_motion_x1 = (Dispatch_device_base::Joint_motion)tp_carrier_status_from_plc_to_dispatch->m_actual_joint_motion_x1; m_actual_joint_motion_x2 = (Dispatch_device_base::Joint_motion)tp_carrier_status_from_plc_to_dispatch->m_actual_joint_motion_x2; memcpy(m_actual_error_code, tp_carrier_status_from_plc_to_dispatch->m_actual_error_code, 50); memcpy(m_actual_warning_code, tp_carrier_status_from_plc_to_dispatch->m_actual_warning_code, 50); m_actual_error_description = (char*)(tp_carrier_status_from_plc_to_dispatch->m_actual_error_description-2); update_actual_coordinates_id(); //通过心跳帧来判断通信是否正常 if ( m_last_heartbeat != tp_carrier_status_from_plc_to_dispatch->m_heartbeat ) { m_last_heartbeat = tp_carrier_status_from_plc_to_dispatch->m_heartbeat; m_status_updata_time = std::chrono::system_clock::now(); /* if ( get_device_id() == 2) { // std::this_thread::sleep_for(std::chrono::seconds(1)); // std::cout << " 111111111111111111111111111 = " << 123 << std::endl; std::cout << " huli test :::: " << " get_dispatch_device_status = " << get_dispatch_device_status() << std::endl; // std::cout << " huli test :::: " << " get_device_id() = " << get_device_id() << std::endl; 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; std::cout << " tp_carrier_response_from_plc_to_dispatch->m_respons_key = " << tp_carrier_response_from_plc_to_dispatch->m_respons_key << std::endl; // int b = tp_carrier_status_from_plc_to_dispatch->m_safe_status; // std::cout << " huli test :::: " << " tp_carrier_status_from_plc_to_dispatch->m_safe_status = " << b << std::endl; // int a = tp_carrier_status_from_plc_to_dispatch->m_work_status; // std::cout << " tp_carrier_status_from_plc_to_dispatch->m_work_status = " << a << std::endl; // std::cout << " m_request_key = " << m_request_key << std::endl; // std::cout << " m_respons_key = " << m_respons_key << std::endl; // std::cout << " huli test :::: " << " m_request_x = " << m_request_x << std::endl; // std::cout << " huli test :::: " << " m_respons_x = " << m_respons_x << std::endl; // std::cout << " huli test :::: " << " m_actual_x = " << m_actual_x << std::endl; // std::cout << " m_actual_device_status = " << m_actual_device_status << std::endl; // std::cout << " 111111111111111111111111111 = " << 456 << std::endl; } */ //重连之后,搬运器状态 E_DISCONNECT ->> E_THREE_LEVEL_WORK if ( m_dispatch_device_status == Dispatch_device_base::DISPATCH_DEVICE_DISCONNECT ) { m_dispatch_device_status = Dispatch_device_base::DISPATCH_DEVICE_THREE_LEVEL_WORK; } } else if(std::chrono::system_clock::now() - m_status_updata_time > std::chrono::milliseconds(COMMUNICATION_OVER_TIME_MS)) { m_dispatch_device_status = Dispatch_device_base::DISPATCH_DEVICE_DISCONNECT; } //else 继续等待,直到消息刷新或者超时. return Error_code::SUCCESS; } //从内存中读数据到任务单, 子类必须重载 Error_manager Carrier::check_and_read_memory_to_task(std::shared_ptr p_task) { Dispatch_communication::get_instance_references().communication_start(); //检查任务类型， if (p_task->get_task_type() != Task_Base::Task_type::CARRIER_TASK) { return Error_manager(Error_code::CARRIER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR, "Carrier::check_task_type get_task_type() != CARRIER_TASK "); } else { if ( m_respons_key == m_request_key && m_respons_status != RESPONS_WORKING ) { Carrier_task* tp_carrier_task = (Carrier_task*)p_task.get(); std::unique_lock t_lock1(tp_carrier_task->m_lock); tp_carrier_task->m_respons_key = m_respons_key; tp_carrier_task->m_respons_status = (Carrier_task::Respons_status)m_respons_status; tp_carrier_task->m_respons_x = m_respons_x; tp_carrier_task->m_respons_y = m_respons_y; tp_carrier_task->m_respons_z = m_respons_z; tp_carrier_task->m_respons_y1 = m_respons_y1; tp_carrier_task->m_respons_y2 = m_respons_y2; tp_carrier_task->m_respons_clamp_motion = (Carrier_task::Clamp_motion)m_respons_clamp_motion; tp_carrier_task->m_respons_joint_motion_x = (Carrier_task::Joint_motion)m_respons_joint_motion_x; tp_carrier_task->m_respons_joint_motion_y = (Carrier_task::Joint_motion)m_respons_joint_motion_y; tp_carrier_task->m_respons_space_id = m_respons_space_id; tp_carrier_task->m_respons_floor_id = m_respons_floor_id; tp_carrier_task->m_respons_wheelbase = m_respons_wheelbase; //如果故障,则添加错误码 if ( m_respons_status == RESPONS_MINOR_ERROR || m_respons_status == RESPONS_CRITICAL_ERROR ) { //添加错误码 Error_manager t_error(CARRIER_RESPONS_ERROR, MINOR_ERROR, "m_respons_status is error"); tp_carrier_task->set_task_error_manager(t_error); } // if ( get_device_id() == 0 ) // { // std::cout << " huli test :::: " << " {{{{{{{{{{{{{{{{{{{{{ 33333333333333333333333333333333333333333 }}}}}}}}}}}}}}}}}}}}}}}}}}= " << 333 << std::endl; // std::cout << " huli test :::: " << " m_request_key = " << m_request_key << std::endl; // std::cout << " huli test :::: " << " m_request_x = " << m_request_x << std::endl; // std::cout << " huli test :::: " << " m_request_y = " << m_request_y << std::endl; // std::cout << " huli test :::: " << " m_request_z = " << m_request_z << std::endl; // std::cout << " huli test :::: " << " m_request_clamp_motion = " << m_request_clamp_motion << std::endl; // std::cout << " huli test :::: " << " m_request_wheelbase = " << m_request_wheelbase << std::endl; // std::cout << " huli test :::: " << " m_request_y1 = " << m_request_y1 << std::endl; // std::cout << " huli test :::: " << " m_request_y2 = " << m_request_y2 << std::endl; // std::cout << " m_request_joint_motion_x = " << m_request_joint_motion_x << std::endl; // // std::cout << " huli test :::: " << " m_respons_key = " << m_respons_key << std::endl; // std::cout << " huli test :::: " << " m_respons_status = " << m_respons_status << std::endl; // std::cout << " huli test :::: " << " m_respons_x = " << m_respons_x << std::endl; // std::cout << " huli test :::: " << " m_respons_y = " << m_respons_y << std::endl; // std::cout << " huli test :::: " << " m_respons_z = " << m_respons_z << std::endl; // std::cout << " huli test :::: " << " m_respons_clamp_motion = " << m_respons_clamp_motion << std::endl; // std::cout << " huli test :::: " << " m_respons_wheelbase = " << m_respons_wheelbase << std::endl; // std::cout << " huli test :::: " << " m_respons_y1 = " << m_respons_y1 << std::endl; // std::cout << " huli test :::: " << " m_respons_y2 = " << m_respons_y2 << std::endl; // std::cout << " m_respons_joint_motion_x = " << m_respons_joint_motion_x << std::endl; // // std::cout << " m_actual_device_status = " << m_actual_device_status << std::endl; // std::cout << " huli test :::: " << " m_actual_load_status = " << m_actual_load_status << std::endl; // std::cout << " huli test :::: " << " m_actual_x = " << m_actual_x << std::endl; // std::cout << " huli test :::: " << " m_actual_y = " << m_actual_y << std::endl; // std::cout << " huli test :::: " << " m_actual_z = " << m_actual_z << std::endl; // std::cout << " huli test :::: " << " m_actual_y1 = " << m_actual_y1 << std::endl; // std::cout << " huli test :::: " << " m_actual_y2 = " << m_actual_y2 << std::endl; // std::cout << " huli test :::: " << " m_actual_clamp_motion1 = " << m_actual_clamp_motion1 << std::endl; // std::cout << " huli test :::: " << " {{{{{{{{{{{{{{{{{{{{{{{{{{{{ 44444444444444444444444444444444444444444 }}}}}}}}}}}}}}}}}}}}}}}}= " << 444 << std::endl; // // } return Error_code::SUCCESS; } //返回没有收到数据 else { return Error_code::NODATA; } } return Error_code::SUCCESS; } //取消下发的指令 Error_manager Carrier::cancel_command() { //以后再写 need programe //目前调度和plc的通信指令做的很简单,没有暂停和急停 复位等操作. //这里先空着,以后再写. //调度模块单方面销毁任务, 不管底层plc的执行情况, 也不去告知plc任务取消. return Error_code::SUCCESS; } //更新真实 空间位置的id. Error_manager Carrier::update_actual_coordinates_id() { Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer(); float t_distance_min = 99999999999999999; float t_distance_current = 0; int t_index_min = 0; //找出最近的坐标点 for (auto iter = tp_dispatch_coordinates->m_packspace_coordinates_map.begin(); iter != tp_dispatch_coordinates->m_packspace_coordinates_map.end(); ++iter) { //只算x轴和z轴 t_distance_current = (m_actual_x - iter->second.x)*(m_actual_x - iter->second.x) + (m_actual_z - iter->second.z)*(m_actual_z - iter->second.z); if ( t_distance_current < t_distance_min ) { t_distance_min = t_distance_current; t_index_min = iter->first; } } m_actual_coordinates_id = t_index_min; //更新空间锁, 只是 覆盖写入加锁. if ( m_actual_coordinates_id != 0 ) { m_actual_coordinates_rows = (m_actual_coordinates_id-1) / Dispatch_coordinates::get_instance_references().m_space_lock_columns; m_actual_coordinates_columns = (m_actual_coordinates_id-1) % Dispatch_coordinates::get_instance_references().m_space_lock_columns; //3楼的搬运器不用加空间锁 if ( get_device_id() != 2 ) { Dispatch_coordinates::get_instance_references().set_space_lock_for_carrier(m_actual_coordinates_rows, m_actual_coordinates_columns, m_device_id); } } return Error_code::SUCCESS; }