puai_dispatch/dispatch/carrier.cpp

//
// Created by huli on 2021/3/12.
//

#include "carrier.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 = 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;

}

Carrier::~Carrier()
{

}

//检查任务类型, 子类必须重载, 用来检查输入的任务是否为子类所需的.
Error_manager Carrier::check_task_type(std::shared_ptr<Task_Base> 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;
}


//把任务单写入到内存中, 子类必须重载
Error_manager Carrier::write_task_to_memory(std::shared_ptr<Task_Base> 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
	{
		std::unique_lock<std::mutex> t_lock(m_lock);

		Carrier_task* tp_carrier_task = (Carrier_task*)p_task.get();
		std::unique_lock<std::mutex> 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<std::mutex> t_lock1(Dispatch_communication::get_instance_references().m_data_lock);
	std::unique_lock<std::mutex> t_lock(m_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];

	//	int heartbeat = tp_carrier_status_from_plc_to_dispatch->m_heartbeat;
	//	std::cout << " huli test :::: " << " heartbeat = " << heartbeat << std::endl;

	//通过心跳帧来判断通信是否正常
	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();

//		int safe_status = tp_carrier_status_from_plc_to_dispatch->m_safe_status;
//		std::cout << " huli test :::: " << " m_safe_status = " << safe_status << std::endl;
//		int work_status = tp_carrier_status_from_plc_to_dispatch->m_work_status;
//		std::cout << " huli test :::: " << " m_work_status = " << work_status << std::endl;

		//设备异常  //注注注注注注注注意了, ==的优先级比&要高.
		if ( (tp_carrier_status_from_plc_to_dispatch->m_safe_status & 0x02) == 0 )
		{
			m_actual_device_status = Dispatch_device_base::DEVICE_EMERGENCY_STOP;
			m_dispatch_device_status = Dispatch_device_base::E_FAULT;
		}
		else if ( (tp_carrier_status_from_plc_to_dispatch->m_safe_status & 0x01) == 0 )
		{
			m_actual_device_status = Dispatch_device_base::DEVICE_FAULT;
			m_dispatch_device_status = Dispatch_device_base::E_FAULT;
		}
		else if ( (tp_carrier_status_from_plc_to_dispatch->m_safe_status & 0x08) == 0 )
		{
			m_actual_device_status = Dispatch_device_base::DEVICE_COLLISION;
			m_dispatch_device_status = Dispatch_device_base::E_FAULT;
		}
			//正常状态
		else
		{
			if (tp_carrier_status_from_plc_to_dispatch->m_work_status == 1)
			{
				m_actual_device_status = Dispatch_device_base::DEVICE_WORKING;
			}
			else if(tp_carrier_status_from_plc_to_dispatch->m_work_status == 2)
			{
				m_actual_device_status = Dispatch_device_base::DEVICE_READY;
			}
			else if(tp_carrier_status_from_plc_to_dispatch->m_work_status == 0)
			{
				m_actual_device_status = Dispatch_device_base::DEVICE_UNKNOWN;
			}

			//故障恢复之后   E_FAULT  ->>  E_THREE_LEVEL_WORK
			if ( m_dispatch_device_status == Dispatch_device_base::E_FAULT )
			{
				m_dispatch_device_status = Dispatch_device_base::E_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;
		if ( m_device_id == 2 )
		{
			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();

		//重连之后,搬运器状态   E_DISCONNECT  ->>  E_THREE_LEVEL_WORK
		if ( m_dispatch_device_status == Dispatch_device_base::E_DISCONNECT )
		{
			m_dispatch_device_status = Dispatch_device_base::E_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::E_DISCONNECT;
	}
	//else 继续等待,直到消息刷新或者超时.

	return Error_code::SUCCESS;
}
//从内存中读数据到任务单, 子类必须重载
Error_manager Carrier::check_and_read_memory_to_task(std::shared_ptr<Task_Base> p_task)
{


	std::cout << " huli test :::: " << " ((((((((((((((((((((((((((333333333333333333 = " << 333 << std::endl;
	std::cout << " huli test :::: " << " m_request_key = " << m_request_key << 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 :::: " << " )))))))))))))))))))))))))))))))4444444444444 = " << 444 << std::endl;




		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
	{
		std::unique_lock<std::mutex> t_lock(m_lock);
		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<std::mutex> 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);
			}

			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 << " 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 << " 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();

	if ( get_device_id() == 2 )
	{
		for (auto iter = tp_dispatch_coordinates->m_carrier_coordinates.begin(); iter != tp_dispatch_coordinates->m_carrier_coordinates.end(); ++iter)
		{
			if ( Common_data::approximate_difference(m_actual_x, iter->second.x, 100) &&
				 Common_data::approximate_difference(m_actual_y, iter->second.y, 500)  &&
				 Common_data::approximate_difference(tp_dispatch_coordinates->m_carrier_3th_floor_z, iter->second.z, 200) )
			{
				m_actual_coordinates_id = iter->first;
			}
		}
	}
	else
	{
		for (auto iter = tp_dispatch_coordinates->m_carrier_coordinates.begin(); iter != tp_dispatch_coordinates->m_carrier_coordinates.end(); ++iter)
		{
			if ( Common_data::approximate_difference(m_actual_x, iter->second.x, 100) &&
				 Common_data::approximate_difference(m_actual_y, iter->second.y, 500) &&
				 Common_data::approximate_difference(m_actual_z, iter->second.z, 200) )
			{
				m_actual_coordinates_id = iter->first;
			}
		}
	}

	return Error_code::SUCCESS;
}