puai_dispatch/dispatch/catcher.cpp

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

#include "catcher.h"
#include "dispatch_manager.h"

Catcher::Catcher()
{
	m_request_x = 0;				//机器人坐标x轴,
	m_request_y = 0;				//机器人坐标y轴,
	m_request_b = 0;				//机器人坐标b轴, 旋转范围80~280
	m_request_z = 0;				//机器人坐标z轴,
	m_request_space_id = 0;			//机器人空间位置的id.
	m_request_d1 = 0;				//机器人坐标d1轴, 机器人抓车杆纵向移动(前轮抓杆)
	m_request_d2 = 0;				//机器人坐标d2轴, 机器人抓车杆纵向移动(后轮抓杆)
	m_request_wheelbase = 0;		//机器人抓车杆前后轮距.
	m_request_clamp_motion = E_CLAMP_NO_ACTION;		//机器人夹车杆. 0=无动作，1=夹紧，2=松开

	m_respons_status = RESPONS_WORKING;			//指令完成状态, 机器人答复指令, 返回任务完成的情况
	m_respons_x = 0;				//机器人坐标x轴,
	m_respons_y = 0;				//机器人坐标y轴,
	m_respons_b = 0;				//机器人坐标b轴, 旋转范围80~280
	m_respons_z = 0;				//机器人坐标z轴,
	m_respons_space_id = 0;			//机器人空间位置的id.
	m_respons_d1 = 0;				//机器人坐标d1轴, 机器人抓车杆纵向移动(前轮抓杆)
	m_respons_d2 = 0;				//机器人坐标d2轴, 机器人抓车杆纵向移动(后轮抓杆)
	m_respons_wheelbase = 0;		//机器人抓车杆前后轮距.
	m_respons_clamp_motion = E_CLAMP_NO_ACTION;		//机器人夹车杆. 0=无动作，1=夹紧，2=松开

	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_b = 0;					//机器人坐标b轴, 旋转范围80~280
	m_actual_z = 0;					//机器人坐标z轴,
	m_actual_d1 = 0;				//机器人坐标d1轴, 机器人抓车杆纵向移动(前轮抓杆)
	m_actual_d2 = 0;				//机器人坐标d2轴, 机器人抓车杆纵向移动(后轮抓杆)
	m_actual_clamp_motion1 = E_CLAMP_NO_ACTION;		//小跑车夹车杆. 0=无动作，1=夹紧，2=松开
	m_actual_clamp_motion2 = E_CLAMP_NO_ACTION;		//小跑车夹车杆. 0=无动作，1=夹紧，2=松开
	m_actual_clamp_motion3 = E_CLAMP_NO_ACTION;		//小跑车夹车杆. 0=无动作，1=夹紧，2=松开
	m_actual_clamp_motion4 = E_CLAMP_NO_ACTION;		//小跑车夹车杆. 0=无动作，1=夹紧，2=松开

	memset(m_actual_error_code, 0, 50);	//搬运器设备的报警信息位
	memset(m_actual_warning_code, 0, 50);	//升降机设备的报警信息位

	m_catcher_direction = CATCHER_DIRECTION_UNKNOW;
	m_actual_coordinates_id = 0;
	m_actual_coordinates_rows = 0;
	m_actual_coordinates_columns = 0;
}

Catcher::~Catcher()
{

}

//检查任务类型, 子类必须重载, 用来检查输入的任务是否为子类所需的.
Error_manager Catcher::check_task_type(std::shared_ptr<Task_Base> p_task)
{
	//检查任务类型，
	if (p_task->get_task_type() != Task_Base::Task_type::CATCHER_TASK)
	{
		return Error_manager(Error_code::CATCHER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
							 "Catcher::check_task_type  get_task_type() != CATCHER_TASK ");
	}

	return Error_code::SUCCESS;
}

//获取硬件设备的状态, 必须子类继承
Catcher::Hardware_device_status Catcher::get_actual_device_status()
{
	return m_actual_device_status;
}


//把任务单写入到内存中, 子类必须重载
Error_manager Catcher::write_task_to_memory(std::shared_ptr<Task_Base> p_task)
{
	//检查任务类型，
	if (p_task->get_task_type() != Task_Base::Task_type::CATCHER_TASK)
	{
		return Error_manager(Error_code::CATCHER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
							 "Catcher::check_task_type  get_task_type() != CATCHER_TASK ");
	}
	else
	{
		Catcher_task* tp_catcher_task = (Catcher_task*)p_task.get();
		std::unique_lock<std::mutex> t_lock1(tp_catcher_task->m_lock);
		m_request_key = tp_catcher_task->m_request_key;
		m_request_x = tp_catcher_task->m_request_x;
		m_request_y = tp_catcher_task->m_request_y;
		m_request_b = tp_catcher_task->m_request_b;
		m_request_z = tp_catcher_task->m_request_z;
		m_request_space_id = tp_catcher_task->m_request_space_id;
		m_request_d1 = tp_catcher_task->m_request_d1;
		m_request_d2 = tp_catcher_task->m_request_d2;
		m_request_wheelbase = tp_catcher_task->m_request_wheelbase;
		m_request_clamp_motion = (Dispatch_device_base::Clamp_motion)tp_catcher_task->m_request_clamp_motion;

		return Error_code::SUCCESS;
	}
	return Error_code::SUCCESS;
}
//更新设备底层通信数据, 子类必须重载
Error_manager Catcher::update_device_communication()
{
	std::unique_lock<std::mutex> t_lock1(Dispatch_communication::get_instance_references().m_data_lock);

	//请求消息, 调度->plc
	Dispatch_communication::Catcher_request_from_dispatch_to_plc_for_data * tp_catcher_request_from_dispatch_to_plc_for_data =
	& Dispatch_communication::get_instance_references().m_catcher_request_from_dispatch_to_plc_for_data[m_device_id];
	Dispatch_communication::Catcher_request_from_dispatch_to_plc_for_key * tp_catcher_request_from_dispatch_to_plc_for_key =
	& Dispatch_communication::get_instance_references().m_catcher_request_from_dispatch_to_plc_for_key[m_device_id];
	memset(tp_catcher_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_catcher_request_from_dispatch_to_plc_for_key->m_request_key, m_request_key.c_str(), t_size);
	tp_catcher_request_from_dispatch_to_plc_for_data->m_request_x = m_request_x;
	tp_catcher_request_from_dispatch_to_plc_for_data->m_request_y = m_request_y;
	tp_catcher_request_from_dispatch_to_plc_for_data->m_request_b = m_request_b;
	tp_catcher_request_from_dispatch_to_plc_for_data->m_request_z = m_request_z;
	tp_catcher_request_from_dispatch_to_plc_for_data->m_request_clamp_motion = m_request_clamp_motion;
	tp_catcher_request_from_dispatch_to_plc_for_data->m_request_wheelbase = m_request_wheelbase;
	tp_catcher_request_from_dispatch_to_plc_for_data->m_request_d1 = m_request_d1;
	tp_catcher_request_from_dispatch_to_plc_for_data->m_request_d2 = m_request_d2;


	//答复消息, plc->调度
	Dispatch_communication::Catcher_response_from_plc_to_dispatch * tp_catcher_response_from_plc_to_dispatch =
	& Dispatch_communication::get_instance_references().m_catcher_response_from_plc_to_dispatch[m_device_id];
	m_respons_key = (char*) tp_catcher_response_from_plc_to_dispatch->m_respons_key;
	m_respons_status = (Dispatch_device_base::Respons_status)tp_catcher_response_from_plc_to_dispatch->m_respons_status;
	m_respons_x = tp_catcher_response_from_plc_to_dispatch->m_respons_x;
	m_respons_y = tp_catcher_response_from_plc_to_dispatch->m_respons_y;
	m_respons_b = tp_catcher_response_from_plc_to_dispatch->m_respons_b;
	m_respons_z = tp_catcher_response_from_plc_to_dispatch->m_respons_z;
	m_respons_clamp_motion = (Dispatch_device_base::Clamp_motion)tp_catcher_response_from_plc_to_dispatch->m_respons_clamp_motion;
	m_respons_wheelbase = tp_catcher_response_from_plc_to_dispatch->m_respons_wheelbase;
	m_respons_d1 = tp_catcher_response_from_plc_to_dispatch->m_respons_d1;
	m_respons_d2 = tp_catcher_response_from_plc_to_dispatch->m_respons_d2;


	//状态消息, plc->调度
	Dispatch_communication::Catcher_status_from_plc_to_dispatch *tp_catcher_status_from_plc_to_dispatch =
	& Dispatch_communication::get_instance_references().m_catcher_status_from_plc_to_dispatch[m_device_id];

	//设备异常  //注注注注注注注注意了, ==的优先级比&要高.
	if ( (tp_catcher_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_catcher_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_catcher_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_catcher_status_from_plc_to_dispatch->m_work_status == 1)
		{
			m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_WORKING;
		}
		else if(tp_catcher_status_from_plc_to_dispatch->m_work_status == 2)
		{
			m_actual_device_status = Dispatch_device_base::HARDWARE_DEVICE_READY;
		}
		else if(tp_catcher_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_catcher_status_from_plc_to_dispatch->m_actual_load_status;
	m_actual_x = tp_catcher_status_from_plc_to_dispatch->m_actual_x;
	m_actual_y = tp_catcher_status_from_plc_to_dispatch->m_actual_y;
	m_actual_b = tp_catcher_status_from_plc_to_dispatch->m_actual_b;
	m_actual_z = tp_catcher_status_from_plc_to_dispatch->m_actual_z;
	m_actual_d1 = tp_catcher_status_from_plc_to_dispatch->m_actual_d1;
	m_actual_d2 = tp_catcher_status_from_plc_to_dispatch->m_actual_d2;
	m_actual_clamp_motion1 = (Dispatch_device_base::Clamp_motion)tp_catcher_status_from_plc_to_dispatch->m_actual_clamp_motion1;
	m_actual_clamp_motion2 = (Dispatch_device_base::Clamp_motion)tp_catcher_status_from_plc_to_dispatch->m_actual_clamp_motion2;
	m_actual_clamp_motion3 = (Dispatch_device_base::Clamp_motion)tp_catcher_status_from_plc_to_dispatch->m_actual_clamp_motion3;
	m_actual_clamp_motion4 = (Dispatch_device_base::Clamp_motion)tp_catcher_status_from_plc_to_dispatch->m_actual_clamp_motion4;
	memcpy(m_actual_error_code, tp_catcher_status_from_plc_to_dispatch->m_actual_error_code, 50);
	memcpy(m_actual_warning_code, tp_catcher_status_from_plc_to_dispatch->m_actual_warning_code, 50);
	m_actual_error_description = (char*)(tp_catcher_status_from_plc_to_dispatch->m_actual_error_description-2);
	update_actual_coordinates_id();

	//机器手朝向
	if ( m_actual_b >= CATCHER_DIRECTION_POSITIVE_MIN && m_actual_b <= CATCHER_DIRECTION_POSITIVE_MAX )
	{
		m_catcher_direction = CATCHER_DIRECTION_POSITIVE;
	}
	else if ( m_actual_b >= CATCHER_DIRECTION_NEGATIVE_MIN && m_actual_b <= CATCHER_DIRECTION_NEGATIVE_MAX )
	{
		m_catcher_direction = CATCHER_DIRECTION_NEGATIVE;
	}
	else
	{
		m_catcher_direction = CATCHER_DIRECTION_UNKNOW;
	}


	//通过心跳帧来判断通信是否正常
	if ( m_last_heartbeat != tp_catcher_status_from_plc_to_dispatch->m_heartbeat )
	{
		m_last_heartbeat = tp_catcher_status_from_plc_to_dispatch->m_heartbeat;
		m_status_updata_time = std::chrono::system_clock::now();

		//重连之后,搬运器状态   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 Catcher::check_and_read_memory_to_task(std::shared_ptr<Task_Base> p_task)
{

//	std::cout << " huli test :::: " << " ((((((((((((((((((((((((((1111111111111 = " << 111 << 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 :::: " << " ))))))))))))))))))))))))))))2222222222 = " << 222 << std::endl;



	Dispatch_communication::get_instance_references().communication_start();

	//检查任务类型，
	if (p_task->get_task_type() != Task_Base::Task_type::CATCHER_TASK)
	{
		return Error_manager(Error_code::CATCHER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
							 "Catcher::check_task_type  get_task_type() != CATCHER_TASK ");
	}
	else
	{
		if ( m_respons_key == m_request_key && m_respons_status != RESPONS_WORKING )
		{
			Catcher_task* tp_catcher_task = (Catcher_task*)p_task.get();
			std::unique_lock<std::mutex> t_lock1(tp_catcher_task->m_lock);

			tp_catcher_task->m_respons_key = m_respons_key;
			tp_catcher_task->m_respons_status = (Catcher_task::Respons_status)m_respons_status;
			tp_catcher_task->m_respons_x = m_respons_x;
			tp_catcher_task->m_respons_y = m_respons_y;
			tp_catcher_task->m_respons_b = m_respons_b;
			tp_catcher_task->m_respons_z = m_respons_z;
			tp_catcher_task->m_respons_space_id = m_respons_space_id;
			tp_catcher_task->m_respons_d1 = m_respons_d1;
			tp_catcher_task->m_respons_d2 = m_respons_d2;
			tp_catcher_task->m_respons_wheelbase = m_respons_wheelbase;
			tp_catcher_task->m_respons_clamp_motion = (Catcher_task::Clamp_motion)m_respons_clamp_motion;

			//如果故障,则添加错误码
			if ( m_respons_status == RESPONS_MINOR_ERROR || m_respons_status == RESPONS_CRITICAL_ERROR )
			{
				//添加错误码
				Error_manager t_error(CATCHER_RESPONS_ERROR, MINOR_ERROR, "m_respons_status is error");
				tp_catcher_task->set_task_error_manager(t_error);
			}
/*
			std::cout << " huli test :::: " << " [[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[     111111111111111111111111    ]]]]]]]]]]]]]]]]]]]]]]]]]]]] = " << 111 << 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_b = " << m_request_b << 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_d1 = " << m_request_d1 << std::endl;
			std::cout << " huli test :::: " << " m_request_d2 = " << m_request_d2 << 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_b = " << m_respons_b << 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_d1 = " << m_respons_d1 << std::endl;
			std::cout << " huli test :::: " << " m_respons_d2 = " << m_respons_d2 << 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_b = " << m_actual_b << std::endl;
			std::cout << " huli test :::: " << " m_actual_z = " << m_actual_z << std::endl;
			std::cout << " huli test :::: " << " m_actual_d1 = " << m_actual_d1 << std::endl;
			std::cout << " huli test :::: " << " m_actual_d2 = " << m_actual_d2 << std::endl;
			std::cout << " huli test :::: " << " m_actual_clamp_motion1 = " << m_actual_clamp_motion1 << std::endl;

			std::cout << " huli test :::: " << " [[[[[[[[[[[[[[[[[[[[[[[[[[[[   2222222222222222222222222222222222222222  ]]]]]]]]]]]]]]]]]]]]]]]]]]] = " << 222 << std::endl;
*/

			return Error_code::SUCCESS;
		}
		//返回没有收到数据
		else
		{
			return Error_code::NODATA;
		}
	}
	return Error_code::SUCCESS;
}



//取消下发的指令
Error_manager Catcher::cancel_command()
{
	//以后再写 need programe
	//目前调度和plc的通信指令做的很简单,没有暂停和急停 复位等操作.
	//这里先空着,以后再写.
	//调度模块单方面销毁任务, 不管底层plc的执行情况, 也不去告知plc任务取消.

	return Error_code::SUCCESS;
}

//更新真实 空间位置的id.
Error_manager Catcher::update_actual_coordinates_id()
{
	Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();
	float t_distance_min = 999999;
	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;
		Dispatch_coordinates::get_instance_references().set_space_lock_for_catcher(m_actual_coordinates_id, m_device_id);
		//保留3 4楼的空间锁
		Dispatch_coordinates::get_instance_references().catcher_3rd_4st_floor_space_lock(m_actual_coordinates_columns, m_device_id);
	}

	return Error_code::SUCCESS;
}