20210602, 调度流程 取车流程 搬运器

dispatch/dispatch_coordinates.cpp

@@ -523,7 +523,7 @@ Error_manager Dispatch_coordinates::catcher_space_unlock(int catcher_coordinates
 	{
 		int t_coordinates_columns = catcher_coordinates_id - 1400;
 		int temp = (t_coordinates_columns-1)%2;
-		t_coordinates_columns = ((t_coordinates_columns-1)/2) + 4 + temp*2;
+		t_coordinates_columns = 4 + ((t_coordinates_columns-1)/2)*3 + temp*2;
 		catcher_4st_floor_space_lock(t_coordinates_columns, catcher_id);
 	}
 	else

dispatch/dispatch_device_base.cpp

@@ -422,6 +422,51 @@ bool Dispatch_device_base::is_able_excute_pickup()
 	}
 }
 
+//判断是否已经预约了任务, return false没有预约任务,    return true已经预约了任务
+bool Dispatch_device_base::is_has_appoint_task(Dispatch_task_level dispatch_task_level)
+{
+	std::unique_lock<std::mutex> t_lock(m_lock);
+	switch ( dispatch_task_level )
+	{
+		case DISPATCH_TASK_ONE_LEVEL:
+		{
+			if ( m_device_one_level_command_key == "" )
+			{
+			    return false;
+			}
+			else
+			{
+			    return true;
+			}
+			break;
+		}
+		case DISPATCH_TASK_TWO_LEVEL:
+		{
+			if ( m_device_two_level_command_key == "" )
+			{
+				return false;
+			}
+			else
+			{
+				return true;
+			}
+			break;
+		}
+		case DISPATCH_TASK_THREE_LEVEL:
+		{
+			if ( m_device_three_level_command_key == "" )
+			{
+				return false;
+			}
+			else
+			{
+				return true;
+			}
+			break;
+		}
+	}
+	return true;
+}
 //预约任务
 Error_manager Dispatch_device_base::appoint_task(std::string command_key, Common_data::Dispatch_process_type process_type, Dispatch_task_level dispatch_task_level)
 {
@@ -456,6 +501,103 @@ Error_manager Dispatch_device_base::appoint_task(std::string command_key, Common
 	return Error_code::SUCCESS;
 }
 
+//修改任务等级
+Error_manager Dispatch_device_base::change_task_level(Dispatch_task_level dispatch_task_level_source, Dispatch_task_level dispatch_task_level_target)
+{
+	std::string * tp_command_key_source = NULL;
+	Common_data::Dispatch_process_type * tp_process_type_source = NULL;
+	std::shared_ptr<Task_Base> * tp_task_source = NULL;
+	std::string * tp_command_key_target = NULL;
+	Common_data::Dispatch_process_type * tp_process_type_target = NULL;
+	std::shared_ptr<Task_Base> * tp_task_target = NULL;
+	
+	if ( dispatch_task_level_source == dispatch_task_level_target )
+	{
+	    return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
+	    					" Dispatch_device_base::change_task_level PARAMRTER ERROR ");
+	}
+	std::unique_lock<std::mutex> t_lock(m_lock);
+	switch ( dispatch_task_level_source )
+	{
+	    case DISPATCH_TASK_ONE_LEVEL:
+	    {
+			tp_command_key_source = & m_device_one_level_command_key;
+			tp_process_type_source = & m_device_one_level_process_type;
+			tp_task_source = & mp_device_one_level_task;
+	        break;
+	    }
+	    case DISPATCH_TASK_TWO_LEVEL:
+	    {
+			tp_command_key_source = & m_device_two_level_command_key;
+			tp_process_type_source = & m_device_two_level_process_type;
+			tp_task_source = & mp_device_two_level_task;;
+	        break; 
+	    }
+		case DISPATCH_TASK_THREE_LEVEL:
+		{
+			tp_command_key_source = & m_device_three_level_command_key;
+			tp_process_type_source = & m_device_three_level_process_type;
+			tp_task_source = & mp_device_three_level_task;;
+			break;
+		}
+	    default:
+	    {
+			return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
+								 " Dispatch_device_base::change_task_level PARAMRTER ERROR ");
+	        break;   
+	    }    
+	}
+	switch ( dispatch_task_level_target )
+	{
+		case DISPATCH_TASK_ONE_LEVEL:
+		{
+			tp_command_key_target = & m_device_one_level_command_key;
+			tp_process_type_target = & m_device_one_level_process_type;
+			tp_task_target = & mp_device_one_level_task;
+			break;
+		}
+		case DISPATCH_TASK_TWO_LEVEL:
+		{
+			tp_command_key_target = & m_device_two_level_command_key;
+			tp_process_type_target = & m_device_two_level_process_type;
+			tp_task_target = & mp_device_two_level_task;;
+			break;
+		}
+		case DISPATCH_TASK_THREE_LEVEL:
+		{
+			tp_command_key_target = & m_device_three_level_command_key;
+			tp_process_type_target = & m_device_three_level_process_type;
+			tp_task_target = & mp_device_three_level_task;;
+			break;
+		}
+		default:
+		{
+			return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
+								 " Dispatch_device_base::change_task_level PARAMRTER ERROR ");
+			break;
+		}
+	}
+
+	if ( (*tp_task_target).get() == NULL )
+	{
+		*tp_command_key_target = *tp_command_key_source;
+		*tp_process_type_target = *tp_process_type_source;
+		*tp_task_target = *tp_task_source;
+
+		(*tp_command_key_source).clear();
+		(*tp_process_type_source) = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW;
+		(*tp_task_source).reset();
+
+		m_dispatch_device_status = Dispatch_device_base::Dispatch_device_status::DISPATCH_DEVICE_THREE_LEVEL_WORK;
+	}
+	else
+	{
+		return Error_manager(Error_code::DISPATCH_DEVICE_TASK_LEVEL_ERROR, Error_level::MINOR_ERROR,
+							 " Dispatch_device_base::change_task_level PARAMRTER ERROR ");
+	}
+
+	return Error_code::SUCCESS;
+}
 
 Dispatch_device_base::Dispatch_device_status Dispatch_device_base::get_dispatch_device_status()
 {
@@ -547,6 +689,8 @@ void Dispatch_device_base::execute_thread_fun()
 							//这里会通知任务已经释放, 然后销毁任务单,  并降级
 							mp_device_three_level_task->set_task_statu(Task_Base::Task_statu::TASK_FREE);
 							mp_device_three_level_task.reset();
+							m_device_three_level_command_key.clear();
+							m_device_three_level_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW;
 							m_dispatch_device_status = DISPATCH_DEVICE_TWO_LEVEL_WORK;
 						}
 							//任务单重新创建, 调度创建的新的任务,那么回去继续工作
@@ -613,6 +757,8 @@ void Dispatch_device_base::execute_thread_fun()
 							//这里会通知任务已经释放, 然后销毁任务单,  并降级
 							mp_device_two_level_task->set_task_statu(Task_Base::Task_statu::TASK_FREE);
 							mp_device_two_level_task.reset();
+							m_device_two_level_command_key.clear();
+							m_device_two_level_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW;
 							m_dispatch_device_status = DISPATCH_DEVICE_ONE_LEVEL_WORK;
 						}
 							//任务单重新创建, 调度创建的新的任务,那么回去继续工作
@@ -693,6 +839,8 @@ void Dispatch_device_base::execute_thread_fun()
 							//这里会通知任务已经释放, 然后销毁任务单,  并降级
 							mp_device_one_level_task->set_task_statu(Task_Base::Task_statu::TASK_FREE);
 							mp_device_one_level_task.reset();
+							m_device_one_level_command_key.clear();
+							m_device_one_level_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW;
 							m_dispatch_device_status = DISPATCH_DEVICE_READY;
 						}
 							//任务单重新创建, 调度创建的新的任务,那么回去继续工作

dispatch/dispatch_device_base.h

@@ -202,9 +202,15 @@ public://API functions
 	virtual bool is_able_excute_store();
 	//判断是否可以执行取车任务。
 	virtual bool is_able_excute_pickup();
+	//判断是否已经预约了任务, return false没有预约任务,    return true已经预约了任务
+	virtual bool is_has_appoint_task(Dispatch_task_level dispatch_task_level);
 
 	//预约任务
 	virtual Error_manager appoint_task(std::string command_key, Common_data::Dispatch_process_type process_type, Dispatch_task_level dispatch_task_level);
+
+	//修改任务等级
+	virtual Error_manager change_task_level(Dispatch_task_level dispatch_task_level_source, Dispatch_task_level dispatch_task_level_target);
+
 public://get or set member variable
 	Dispatch_device_status get_dispatch_device_status();
 	//获取硬件设备的状态, 必须子类继承

dispatch/dispatch_process.cpp

@@ -640,12 +640,12 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
 					{
 					    //连接避让设备 使得避让机器人进行避让
 						m_dispatch_carrier_node.m_error = m_dispatch_carrier_node.mp_avoid_catcher->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL);
-						if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
+						if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
 						{
-							m_dispatch_catcher_node.m_error = connect_dispatch_catcher(m_dispatch_carrier_node.mp_avoid_catcher, m_dispatch_carrier_node.mp_avoid_catcher_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL);
-							if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
+							m_dispatch_carrier_node.m_error = connect_dispatch_catcher(m_dispatch_carrier_node.mp_avoid_catcher, m_dispatch_carrier_node.mp_avoid_catcher_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL);
+							if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
 							{
-								m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
+								m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
 							}
 						}
 						//else 抓取器的资源自由争抢, 如果没有抢到, 那么重复连接
@@ -673,8 +673,8 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
 		}
 		case DISPATCH_CARRIER_PICKUP_11://机器人释放 空间锁
 		{
-			m_dispatch_catcher_node.m_error = check_task_status(m_dispatch_carrier_node.mp_avoid_catcher_task, m_dispatch_carrier_node.m_dispatch_control_status);
-			if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
+			m_dispatch_carrier_node.m_error = check_task_status(m_dispatch_carrier_node.mp_avoid_catcher_task, m_dispatch_carrier_node.m_dispatch_control_status);
+			if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
 			{
 				tp_dispatch_coordinates->catcher_space_unlock(tp_avoid_catcher->m_actual_coordinates_id, tp_avoid_catcher->get_device_id());
 			}
@@ -729,7 +729,7 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
 		case DISPATCH_CARRIER_PICKUP_14://搬运器 释放空间锁
 		{
 			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
-			if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
+			if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
 			{
 				m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_space_unlock(tp_main_carrier->get_device_id());
 			}
@@ -785,10 +785,10 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
 			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
 			break;
 		}
-		case DISPATCH_CARRIER_PICKUP_22://中跑车 x轴移动, 进行x轴路径检查
+		case DISPATCH_CARRIER_PICKUP_22://中跑车 x轴移动到车位, 进行x轴路径检查
 		{
 			float t_x = tp_dispatch_coordinates->m_carrier_coordinates[m_parkspace_information_optimal.parkingspace_index_id].x;
-			float t_column = (m_parkspace_information_optimal.parkingspace_index_id-1) % tp_dispatch_coordinates->m_space_lock_columns;
+			int t_column = (m_parkspace_information_optimal.parkingspace_index_id-1) % tp_dispatch_coordinates->m_space_lock_columns;
 			if ( Common_data::approximate_difference(tp_main_carrier->m_actual_x, t_x, DISPATCH_DEFAULT_DIFFERENCE) )
 			{
 				//搬运器不用x轴运动, 直接到29步, 去车位取车
@@ -833,12 +833,12 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
 				{
 					//连接避让设备 使得避让机器人进行避让
 					m_dispatch_carrier_node.m_error = m_dispatch_carrier_node.mp_avoid_catcher->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL);
-					if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
+					if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
 					{
-						m_dispatch_catcher_node.m_error = connect_dispatch_catcher(m_dispatch_carrier_node.mp_avoid_catcher, m_dispatch_carrier_node.mp_avoid_catcher_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL);
-						if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
+						m_dispatch_carrier_node.m_error = connect_dispatch_catcher(m_dispatch_carrier_node.mp_avoid_catcher, m_dispatch_carrier_node.mp_avoid_catcher_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL);
+						if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
 						{
-							m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
+							m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
 						}
 					}
 					//else 抓取器的资源自由争抢, 如果没有抢到, 那么重复连接
@@ -862,7 +862,8 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
 				//判断结果
 				if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
 				{
-					float t_x = tp_dispatch_coordinates->m_carrier_coordinates[42].x;
+					//机器人移到34号口
+					float t_x = tp_dispatch_coordinates->m_carrier_coordinates[34].x;
 					catcher_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_avoid_catcher, tp_avoid_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
 					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
 				}
@@ -876,7 +877,8 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
 				//判断结果
 				if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
 				{
-					float t_x = tp_dispatch_coordinates->m_carrier_coordinates[34].x;
+					//机器人移到42号口
+					float t_x = tp_dispatch_coordinates->m_carrier_coordinates[42].x;
 					catcher_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_avoid_catcher, tp_avoid_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
 					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
 				}
@@ -888,7 +890,7 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
 		case DISPATCH_CARRIER_PICKUP_25://机器人释放 空间锁
 		{
 			check_task_status(m_dispatch_carrier_node.mp_avoid_catcher_task, m_dispatch_carrier_node.m_dispatch_control_status);
-			if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
+			if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
 			{
 				tp_dispatch_coordinates->catcher_space_unlock(tp_avoid_catcher->m_actual_coordinates_id, tp_avoid_catcher->get_device_id());
 			}
@@ -896,7 +898,7 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
 		}
 		case DISPATCH_CARRIER_PICKUP_26://空间锁判断, 必须成功
 		{
-			float t_column = (m_parkspace_information_optimal.parkingspace_index_id-1) % tp_dispatch_coordinates->m_space_lock_columns;
+			int t_column = (m_parkspace_information_optimal.parkingspace_index_id-1) % tp_dispatch_coordinates->m_space_lock_columns;
 			int t_avoid_catcher_id = -1;	//需要避让的机器人
 			if ( tp_main_carrier->get_device_id() == 0 && t_column <13-1)
 			{
@@ -945,61 +947,372 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
 			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
 			break;
 		}
-		case DISPATCH_CARRIER_PICKUP_31://小跑车 夹车
+		case DISPATCH_CARRIER_PICKUP_31://把任务从一级升到三级, 准备取车
+		{
+			m_dispatch_carrier_node.m_error = m_dispatch_carrier_node.mp_main_carrier->change_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_THREE_LEVEL);
+			if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
+			{
+				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
+			}
+			else
+			{
+				m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
+			}
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_32://小跑车 夹车
 		{
 			carrier_move_c(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_TIGHT);
 			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
 			break;
 		}
-		case DISPATCH_CARRIER_PICKUP_32:
+		case DISPATCH_CARRIER_PICKUP_33:
 		{
 			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
 			break;
 		}
-		case DISPATCH_CARRIER_PICKUP_33://小跑车 回到中跑车
+		case DISPATCH_CARRIER_PICKUP_34://小跑车 回到中跑车
 		{
 			carrier_move_y(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
 			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
 			break;
 		}
-		case DISPATCH_CARRIER_PICKUP_34:
+		case DISPATCH_CARRIER_PICKUP_35:
 		{
 			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
 			break;
 		}
-		case DISPATCH_CARRIER_PICKUP_35://让中跑车回到电梯井
+		case DISPATCH_CARRIER_PICKUP_36://让中跑车回到电梯井
 		{
 			//如果不在2楼, 那么就要退回电梯井
-			if ( tp_main_carrier->get_device_id() == 0  && tp_main_carrier->m_actual_coordinates_rows != 0)
+			if ( tp_main_carrier->get_device_id() == 0  && (tp_main_carrier->m_actual_coordinates_rows != 0 || m_dispatch_destination == 1100))
 			{
 				carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
 				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
 			}
-			else if ( tp_main_carrier->get_device_id() == 1 && tp_main_carrier->m_actual_coordinates_rows != 0 )
+			else if ( tp_main_carrier->get_device_id() == 1 && (tp_main_carrier->m_actual_coordinates_rows != 0 || m_dispatch_destination == 1107))
 			{
 				carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
 				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
 			}
+			else if ( tp_main_carrier->get_device_id() == 2 )
+			{
+				//3楼搬运器取车后, 不用x轴移动, 直接准备交接
+				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_50);
+			}
 			else
 			{
-				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+999);
+				//不用回电梯井, 直接准备x轴移动到出口上方
+				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_44);
 			}
 			break;
 		}
-		case DISPATCH_CARRIER_PICKUP_36://搬运器 释放空间锁
+		case DISPATCH_CARRIER_PICKUP_37://搬运器 释放空间锁
 		{
 			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
-			if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
+			if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
 			{
 				m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_space_unlock(tp_main_carrier->get_device_id());
 			}
 			break;
 		}
+		case DISPATCH_CARRIER_PICKUP_38://避让机器人 断连
+		{
+			if ( m_dispatch_carrier_node.mp_avoid_catcher.get() != NULL && m_dispatch_carrier_node.mp_avoid_catcher_task.get() != NULL )
+			{
+				disconnect_dispatch_device(m_dispatch_carrier_node.mp_avoid_catcher, m_dispatch_carrier_node.mp_avoid_catcher_task);
+			}
+			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_39://收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动
+		{
+			carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
+			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_40://电梯移动到2楼, (7号出口移动到1楼), 搬运器在电梯里面z轴移动, 不需要判断空间锁
+		{
+			float t_z =0;
+			if ( (m_dispatch_destination == 1107 && tp_main_carrier->get_device_id() == 1) ||
+				 (m_dispatch_destination == 1100 && tp_main_carrier->get_device_id() == 0) )//1楼出口
+			{
+				t_z = tp_dispatch_coordinates->m_carrier_1th_floor_z;
+			}
+			else if ( m_dispatch_destination >1100 && m_dispatch_destination<1107 )
+			{
+				t_z = tp_dispatch_coordinates->m_carrier_2th_floor_z;
+			}
+			else
+			{
+				m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
+				break;//直接跳转到故障
+			}
+			carrier_move_z(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_z);
+			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_41:
+		{
+			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_42://伸出对接，之后中跑车可以x轴移动，电梯不能Z轴移动
+		{
+			if ( tp_main_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK )
+			{
+				carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT);
+				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
+			}
+			else
+			{
+				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2);
+			}
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_43:
+		{
+			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_44://中跑车 x轴移动, 进行x轴路径检查, 加空间锁失败就进入待机位
+		{
+			//need program, 等待抢占机器人资源, 并开始执行取车任务
+//			if ( m_dispatch_catcher_node.m_dispatch_control_status >=999 )
+			//7号出口 特殊处理
 
+			float t_x = tp_dispatch_coordinates->m_carrier_coordinates[m_dispatch_destination].x;
+			int t_terminal = m_dispatch_destination - PASSAGEWAY_ID_BASE;
+			int t_temp = (t_terminal-1)%2;
+			int t_column = 4 + ((t_terminal-1)/2)*3 + t_temp*2 -1;
 
+			if ( Common_data::approximate_difference(tp_main_carrier->m_actual_x, t_x, DISPATCH_DEFAULT_DIFFERENCE) )
+			{
+				//搬运器不用x轴运动, 直接到50步, 准备交接
+				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_50);
+			}
+			else
+			{
+				int t_avoid_catcher_id = -1;	//需要避让的机器人
+				if ( tp_main_carrier->get_device_id() == 0 && t_column <13-1)
+				{
+					m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, tp_main_carrier->m_actual_coordinates_columns,
+																									  tp_main_carrier->m_actual_coordinates_rows, t_column+1,
+																									  tp_main_carrier->get_device_id(), t_avoid_catcher_id);
+				}
+				else if ( tp_main_carrier->get_device_id() == 1 && t_column >3-1)
+				{
+					m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, t_column-1,
+																									  tp_main_carrier->m_actual_coordinates_rows, tp_main_carrier->m_actual_coordinates_columns,
+																									  tp_main_carrier->get_device_id(), t_avoid_catcher_id);
+				}
+				else if ( tp_main_carrier->get_device_id() == 2 )
+				{
+					//3楼搬运器不用加锁, 直接x轴移动
+					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_48);
+					break;//切换流程
+				}
+				else
+				{
+					m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
+					m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
+																	" tp_main_carrier->get_device_id() and m_parkspace_information_optimal.parkingspace_index_id PARAMRTER ERROR ");
+					break;//切换流程
+				}
 
+				//判断结果
+				if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
+				{
+					//左右搬运器加锁成功, 直接x轴移动
+					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_48);
+				}
+				else
+				{
+					//加锁失败, 就移动到待机位.
+					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
+				}
+			}
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_45://中跑车 x轴移动, 移动到待机位
+		{
+			if ( tp_main_carrier->get_device_id() == 0 )
+			{
+				float t_x = tp_dispatch_coordinates->m_carrier_coordinates[2].x;
+				carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_x);
+				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
+			}
+			else if ( tp_main_carrier->get_device_id() == 1 )
+			{
+				float t_x = tp_dispatch_coordinates->m_carrier_coordinates[14].x;
+				carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_x);
+				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
+			}
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_46:
+		{
+			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_47://中跑车 x轴移动, 进行x轴路径检查, 加空间锁失败就 只能无限等待
+		{
+			float t_x = tp_dispatch_coordinates->m_carrier_coordinates[m_dispatch_destination].x;
+			int t_terminal = m_dispatch_destination - PASSAGEWAY_ID_BASE;
+			int t_temp = (t_terminal-1)%2;
+			int t_column = 4 + ((t_terminal-1)/2)*3 + t_temp*2 -1;
 
+			if ( Common_data::approximate_difference(tp_main_carrier->m_actual_x, t_x, DISPATCH_DEFAULT_DIFFERENCE) )
+			{
+				//搬运器不用x轴运动, 直接到29步, 去车位取车
+				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_29);
+			}
+			else
+			{
+				int t_avoid_catcher_id = -1;	//需要避让的机器人
+				if ( tp_main_carrier->get_device_id() == 0 && t_column <13-1)
+				{
+					m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, tp_main_carrier->m_actual_coordinates_columns,
+																									  tp_main_carrier->m_actual_coordinates_rows, t_column+1,
+																									  tp_main_carrier->get_device_id(), t_avoid_catcher_id);
+				}
+				else if ( tp_main_carrier->get_device_id() == 1 && t_column >3-1)
+				{
+					m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_try_space_lock(tp_main_carrier->m_actual_coordinates_rows, t_column-1,
+																									  tp_main_carrier->m_actual_coordinates_rows, tp_main_carrier->m_actual_coordinates_columns,
+																									  tp_main_carrier->get_device_id(), t_avoid_catcher_id);
+				}
+				else if ( tp_main_carrier->get_device_id() == 2 )
+				{
+					//3楼搬运器不用加锁, 直接x轴移动
+					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_48);
+					break;//切换流程
+				}
+				else
+				{
+					m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
+					m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
+																	" tp_main_carrier->get_device_id() and m_parkspace_information_optimal.parkingspace_index_id PARAMRTER ERROR ");
+					break;//切换流程
+				}
 
+				//判断结果
+				if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
+				{
+					//左右搬运器加锁成功, 直接x轴移动
+					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
+				}
+				//else 无限等待
+			}
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_48://中跑车 x轴移动, 移动到取车口的上方2楼处
+		{
+			float t_x = tp_dispatch_coordinates->m_carrier_coordinates[m_dispatch_destination].x;
+			carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_x);
+			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_49:
+		{
+			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_50://小跑车 松开夹杆
+		{
+			carrier_move_c(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_LOOSE);
+			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_51:
+		{
+			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_52://等待机器人把车从搬运器上面取走
+		{
+//need program, 等待机器人把车从搬运器上面取走
+			if ( m_dispatch_catcher_node.m_dispatch_control_status >= 999 )
+			{
+				if ( tp_main_carrier->get_device_id() == 2 )
+				{
+				    //进入3楼搬运器取车的特殊流程, 跟随和避让
+//					m_dispatch_carrier_node.m_dispatch_control_status = 999;
+				}
+				else
+				{
+					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
+				}
+			}
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_53://搬运器退回电梯井
+		{
+			if ( tp_main_carrier->get_device_id() == 0 )
+			{
+				carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
+				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
+			}
+			else if ( tp_main_carrier->get_device_id() == 1 )
+			{
+				carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
+				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
+			}
+			else
+			{
+				m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
+				m_dispatch_carrier_node.m_error = Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
+																" tp_main_carrier->get_device_id() PARAMRTER ERROR ");
+			}
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_54://搬运器 释放空间锁
+		{
+			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
+			if ( m_dispatch_carrier_node.m_error == Error_code::SUCCESS )
+			{
+				m_dispatch_carrier_node.m_error = tp_dispatch_coordinates->carrier_space_unlock(tp_main_carrier->get_device_id());
+			}
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_55://收回对接，之后中跑车固定在电梯上不能X轴移动，电梯可以Z轴移动
+		{
+			//如果后面有一级存车任务, 那么就跳过
+			if ( tp_main_carrier->is_has_appoint_task(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL))
+			{
+				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+2);
+			}
+			else
+			{
+				carrier_joint_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
+				m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
+			}
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_56:
+		{
+			check_task_status(m_dispatch_carrier_node.mp_main_carrier_task, m_dispatch_carrier_node.m_dispatch_control_status);
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_57://主搬运器 断连
+		{
+			disconnect_dispatch_device(m_dispatch_carrier_node.mp_main_carrier, m_dispatch_carrier_node.mp_main_carrier_task);
+			m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_END);
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_58:
+		{
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_59:
+		{
+			break;
+		}
+		case DISPATCH_CARRIER_PICKUP_END:
+		{
+			m_dispatch_carrier_node.m_error = Error_code::SUCCESS;
+			break;
+		}
 		default:
 		{
 

dispatch/dispatch_process.h

@@ -152,6 +152,19 @@ public:
 		DISPATCH_CARRIER_PICKUP_47				= 247,
 		DISPATCH_CARRIER_PICKUP_48				= 248,
 		DISPATCH_CARRIER_PICKUP_49				= 249,
+		DISPATCH_CARRIER_PICKUP_50				= 250,
+		DISPATCH_CARRIER_PICKUP_51				= 251,
+		DISPATCH_CARRIER_PICKUP_52				= 252,
+		DISPATCH_CARRIER_PICKUP_53				= 253,
+		DISPATCH_CARRIER_PICKUP_54				= 254,
+		DISPATCH_CARRIER_PICKUP_55				= 255,
+		DISPATCH_CARRIER_PICKUP_56				= 256,
+		DISPATCH_CARRIER_PICKUP_57				= 257,
+		DISPATCH_CARRIER_PICKUP_58				= 258,
+		DISPATCH_CARRIER_PICKUP_59				= 259,
+
+		DISPATCH_CARRIER_PICKUP_END				= 299,
+
 
 		DISPATCH_CATCHER_PICKUP_START			= 300, 	//取车流程 开始
 		DISPATCH_CATCHER_PICKUP_1				= 301,

main.cpp

@@ -69,13 +69,10 @@ public:
 
 int main(int argc,char* argv[])
 {
-//	std::shared_ptr<AAA> aaa(new BBB);
-//	aaa->a = 10;
-//	std::cout << " huli test :::: " << " aaa.size() = " << aaa.use_count()<< std::endl;
-//	std::cout << " huli test :::: " << " aaa->a = " << aaa->a<< std::endl;
-//
-//	BBB* bbb = (BBB*)aaa.get();
-//
+//	int t_terminal = 6;
+//	int t_temp = (t_terminal-1)%2;
+//	int t_column = 4 + ((t_terminal-1)/2)*3 + t_temp*2 ;
+//std::cout << " huli test :::: " << " t_column = " << t_column << std::endl;
 //
 //	return 0;