|
@@ -273,7 +273,7 @@ void Dispatch_process::Main()
|
|
|
{
|
|
|
std::this_thread::sleep_for(std::chrono::microseconds(1));
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
|
|
- std::this_thread::sleep_for(std::chrono::seconds(1));
|
|
|
+// std::this_thread::sleep_for(std::chrono::seconds(1));
|
|
|
// std::cout << " huli test :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: " << " m_dispatch_process_status = " << m_dispatch_process_status << std::endl;
|
|
|
|
|
|
switch ( (Dispatch_process_status)m_dispatch_process_status )
|
|
@@ -314,28 +314,30 @@ void Dispatch_process::Main()
|
|
|
m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_READY;
|
|
|
m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_READY;
|
|
|
m_dispatch_process_status = DISPATCH_PROCESS_WORKING_STORE;
|
|
|
+
|
|
|
+ //根据设备选择, 来设置终点
|
|
|
+ if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 2 )
|
|
|
+ {
|
|
|
+ m_parkspace_information_optimal = m_parkspace_information_temporary;
|
|
|
+ }
|
|
|
+ else if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 0 && m_temporary_cache_flag == false )
|
|
|
+ {
|
|
|
+ m_parkspace_information_optimal = m_parkspace_information_left;
|
|
|
+ }
|
|
|
+ else if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 0 && m_temporary_cache_flag == true)
|
|
|
+ {
|
|
|
+ m_parkspace_information_optimal = m_parkspace_information_temporary;
|
|
|
+ }
|
|
|
+ else if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 1 && m_temporary_cache_flag == false )
|
|
|
+ {
|
|
|
+ m_parkspace_information_optimal = m_parkspace_information_right;
|
|
|
+ }
|
|
|
+ else if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 1 && m_temporary_cache_flag == true)
|
|
|
+ {
|
|
|
+ m_parkspace_information_optimal = m_parkspace_information_temporary;
|
|
|
+ }
|
|
|
}
|
|
|
- //根据设备选择, 来设置终点
|
|
|
- if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 2 )
|
|
|
- {
|
|
|
- m_parkspace_information_optimal = m_parkspace_information_temporary;
|
|
|
- }
|
|
|
- else if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 0 && m_temporary_cache_flag == false )
|
|
|
- {
|
|
|
- m_parkspace_information_optimal = m_parkspace_information_left;
|
|
|
- }
|
|
|
- else if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 0 && m_temporary_cache_flag == true)
|
|
|
- {
|
|
|
- m_parkspace_information_optimal = m_parkspace_information_temporary;
|
|
|
- }
|
|
|
- else if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 1 && m_temporary_cache_flag == false )
|
|
|
- {
|
|
|
- m_parkspace_information_optimal = m_parkspace_information_right;
|
|
|
- }
|
|
|
- else if ( m_dispatch_carrier_node.mp_main_carrier->get_device_id() == 1 && m_temporary_cache_flag == true)
|
|
|
- {
|
|
|
- m_parkspace_information_optimal = m_parkspace_information_temporary;
|
|
|
- }
|
|
|
+
|
|
|
}
|
|
|
//else 一直等待资源分配
|
|
|
break;
|
|
@@ -518,6 +520,7 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
|
|
|
{
|
|
|
//通知机器人提前开始
|
|
|
m_dispatch_catcher_node.m_dispatch_control_start_flag = true;
|
|
|
+ m_dispatch_catcher_node.m_following_flag = false;
|
|
|
}
|
|
|
//连接搬运器, 创建新的任务单 与设备建立连接, 只能成功, 失败就要进入故障处理
|
|
|
m_dispatch_carrier_node.m_error = m_dispatch_carrier_node.mp_main_carrier->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_ONE_LEVEL);
|
|
@@ -621,6 +624,7 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
|
|
|
case DISPATCH_CARRIER_PICKUP_8://让中跑车回到电梯井, 进行x轴路径检查
|
|
|
{
|
|
|
float t_z = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id].z;
|
|
|
+
|
|
|
if ( Common_data::approximate_difference(tp_main_carrier->m_actual_z, t_z, DISPATCH_DEFAULT_DIFFERENCE) )
|
|
|
{
|
|
|
//直接到 20步 , 准备同层移动到车位
|
|
@@ -1402,6 +1406,7 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
|
|
|
t_following_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination+1].x;
|
|
|
}
|
|
|
carrier_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_main_carrier, tp_main_carrier_task, tp_dispatch_coordinates, t_following_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_71:
|
|
@@ -1502,6 +1507,29 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+ std::cout << " huli test :::: " << " ======================================================================================== = " << 1 << std::endl;
|
|
|
+ std::cout << " huli test :::: " << " m_dispatch_carrier_node.m_dispatch_control_status = " << m_dispatch_carrier_node.m_dispatch_control_status << std::endl;
|
|
|
+ if ( tp_main_carrier != NULL )
|
|
|
+ {
|
|
|
+ std::cout << " huli test :::: " << " tp_main_carrier->get_device_id() = " << tp_main_carrier->get_device_id() << std::endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ std::cout << " huli test :::: " << " tp_main_carrier->get_device_id() = " << -8888 << std::endl;
|
|
|
+ }
|
|
|
+ if ( tp_avoid_catcher != NULL )
|
|
|
+ {
|
|
|
+ std::cout << " huli test :::: " << " tp_avoid_catcher->get_device_id() = " << tp_avoid_catcher->get_device_id() << std::endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ std::cout << " huli test :::: " << " tp_avoid_catcher->get_device_id() = " << -8888 << std::endl;
|
|
|
+ }
|
|
|
+// char cr;
|
|
|
+// std::cin >> cr ;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
Error_manager t_catcher_error;
|
|
|
Catcher * tp_main_catcher = NULL;
|
|
|
Catcher_task * tp_main_catcher_task = NULL;
|
|
@@ -1637,9 +1665,11 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
|
|
|
m_dispatch_catcher_node.mp_following_carrier = Dispatch_manager::get_instance_references().m_carrier_map[2];
|
|
|
}
|
|
|
m_dispatch_catcher_node.m_error = m_dispatch_catcher_node.mp_following_carrier->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL);
|
|
|
+ std::cout << " huli test :::: " << " m_dispatch_catcher_node.m_error = " << m_dispatch_catcher_node.m_error.to_string() << std::endl;
|
|
|
if ( m_dispatch_catcher_node.m_error == Error_code::SUCCESS )
|
|
|
{
|
|
|
m_dispatch_catcher_node.m_error = connect_dispatch_carrier(m_dispatch_catcher_node.mp_following_carrier, m_dispatch_catcher_node.mp_following_carrier_task, Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL);
|
|
|
+ std::cout << " huli test :::: " << " m_dispatch_catcher_node.m_error = " << m_dispatch_catcher_node.m_error.to_string() << std::endl;
|
|
|
if ( m_dispatch_catcher_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);
|
|
@@ -1660,7 +1690,7 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
|
|
|
int temp = (t_terminal-1)%2;
|
|
|
int t_column = 4 + ((t_terminal-1)/2)*3 + temp*2 -1;
|
|
|
|
|
|
- if ( tp_avoid_catcher->m_actual_coordinates_columns < t_column )
|
|
|
+ if ( tp_main_catcher->m_actual_coordinates_columns < t_column )
|
|
|
{
|
|
|
m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_try_space_lock(2, tp_main_catcher->m_actual_coordinates_columns,
|
|
|
2, t_column+2,
|
|
@@ -1682,7 +1712,17 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
|
|
|
}
|
|
|
case DISPATCH_CATCHER_PICKUP_10://机器手调整到 对接搬运器的姿态
|
|
|
{
|
|
|
- float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x;
|
|
|
+ float t_x = 0;
|
|
|
+ // 2楼交接, 在出口终点x交接 3楼交接, 在取车车位起点x交接,
|
|
|
+ if ( m_dispatch_catcher_node.m_following_flag )
|
|
|
+ {
|
|
|
+ t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_dispatch_destination].x;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[m_parkspace_information_optimal.parkingspace_index_id].x;
|
|
|
+ }
|
|
|
+
|
|
|
float t_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2);
|
|
|
if ( Common_data::approximate_difference(tp_main_catcher->m_actual_x, t_x, DISPATCH_DEFAULT_DIFFERENCE) &&
|
|
|
Common_data::approximate_difference(tp_main_catcher->m_actual_y, t_y, DISPATCH_DEFAULT_DIFFERENCE))
|
|
@@ -1736,9 +1776,28 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
|
|
|
}
|
|
|
case DISPATCH_CATCHER_PICKUP_12://机器手 准备下降抓车之前, 等待搬运器就位, 然后把一级任务升到三级
|
|
|
{
|
|
|
- //等待搬运器就位, 把一级任务升到三级
|
|
|
- if ( m_dispatch_carrier_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_52 )
|
|
|
+ //2楼交接时, 机器人先等待, 再升级, 3楼交接时, 机器人先升级, 再等待,
|
|
|
+ if ( m_dispatch_catcher_node.m_following_flag )
|
|
|
+ {
|
|
|
+ // 2楼交接, 等待搬运器准备好, 机器人再升级
|
|
|
+ //等待搬运器就位, 把一级任务升到三级
|
|
|
+ if ( m_dispatch_carrier_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_52 )
|
|
|
+ {
|
|
|
+ m_dispatch_catcher_node.m_error = m_dispatch_catcher_node.mp_main_catcher->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_catcher_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);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //else 无限等待
|
|
|
+ }
|
|
|
+ else
|
|
|
{
|
|
|
+ //3楼交接, 机器人直接升级, 需要锁定位置, 不能为其他流程避让
|
|
|
m_dispatch_catcher_node.m_error = m_dispatch_catcher_node.mp_main_catcher->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_catcher_node.m_error == Error_code::SUCCESS )
|
|
|
{
|
|
@@ -1749,22 +1808,44 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
|
|
|
m_dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CONTROL_FAULT;
|
|
|
}
|
|
|
}
|
|
|
- //else 无限等待
|
|
|
+
|
|
|
break;
|
|
|
}
|
|
|
case DISPATCH_CATCHER_PICKUP_13://机器手 z轴下降, 空间加锁
|
|
|
{
|
|
|
- //2楼交接的空间已经由搬运器加锁, 机器人下降时就不判断了, 强制加锁即可.
|
|
|
- if ( m_dispatch_catcher_node.m_following_flag )//去3楼取车不用加锁
|
|
|
+ //2楼交接时, 机器人先等待, 再升级, 3楼交接时, 机器人先升级, 再等待,
|
|
|
+ if ( m_dispatch_catcher_node.m_following_flag )
|
|
|
{
|
|
|
- int t_terminal = m_dispatch_destination - 1100;
|
|
|
- int temp = (t_terminal-1)%2;
|
|
|
- int t_column = 4 + ((t_terminal-1)/2)*3 + temp*2 -1;
|
|
|
- m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_force_space_lock(0, t_column-1,
|
|
|
- 2, t_column+1,
|
|
|
- tp_main_catcher->get_device_id());
|
|
|
+ //2楼交接的空间已经由搬运器加锁, 机器人下降时就不判断了, 强制加锁即可.
|
|
|
+ if ( m_dispatch_catcher_node.m_following_flag )//去3楼取车不用加锁
|
|
|
+ {
|
|
|
+ int t_terminal = m_dispatch_destination - 1100;
|
|
|
+ int temp = (t_terminal-1)%2;
|
|
|
+ int t_column = 4 + ((t_terminal-1)/2)*3 + temp*2 -1;
|
|
|
+ m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_force_space_lock(0, t_column-1,
|
|
|
+ 2, t_column+1,
|
|
|
+ tp_main_catcher->get_device_id());
|
|
|
+ }
|
|
|
+ m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if ( m_dispatch_carrier_node.m_dispatch_control_status >= Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_52 )
|
|
|
+ {
|
|
|
+ //2楼交接的空间已经由搬运器加锁, 机器人下降时就不判断了, 强制加锁即可.
|
|
|
+ if ( m_dispatch_catcher_node.m_following_flag )//去3楼取车不用加锁
|
|
|
+ {
|
|
|
+ int t_terminal = m_dispatch_destination - 1100;
|
|
|
+ int temp = (t_terminal-1)%2;
|
|
|
+ int t_column = 4 + ((t_terminal-1)/2)*3 + temp*2 -1;
|
|
|
+ m_dispatch_catcher_node.m_error = tp_dispatch_coordinates->catcher_force_space_lock(0, t_column-1,
|
|
|
+ 2, t_column+1,
|
|
|
+ tp_main_catcher->get_device_id());
|
|
|
+ }
|
|
|
+ m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
|
|
|
+ }
|
|
|
+ //else 无限等待
|
|
|
}
|
|
|
- m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
|
|
|
break;
|
|
|
}
|
|
|
case DISPATCH_CATCHER_PICKUP_14://机器手 z轴下降
|
|
@@ -1977,7 +2058,7 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
|
|
|
check_task_status(m_dispatch_catcher_node.mp_main_catcher_task, m_dispatch_catcher_node.m_dispatch_control_status);
|
|
|
break;
|
|
|
}
|
|
|
- case DISPATCH_CATCHER_PICKUP_36://主搬运器 断连
|
|
|
+ case DISPATCH_CATCHER_PICKUP_36://主机器人 断连
|
|
|
{
|
|
|
disconnect_dispatch_device(m_dispatch_catcher_node.mp_main_catcher, m_dispatch_catcher_node.mp_main_catcher_task);
|
|
|
if ( m_dispatch_catcher_node.m_following_flag )
|
|
@@ -1999,25 +2080,9 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
|
|
|
}
|
|
|
}
|
|
|
|
|
|
- std::cout << " huli test :::: " << " =========================================================== = " << 1 << std::endl;
|
|
|
- std::cout << " huli test :::: " << " m_dispatch_carrier_node.m_dispatch_control_status = " << m_dispatch_carrier_node.m_dispatch_control_status << std::endl;
|
|
|
- if ( tp_main_carrier != NULL )
|
|
|
- {
|
|
|
- std::cout << " huli test :::: " << " tp_main_carrier->get_device_id() = " << tp_main_carrier->get_device_id() << std::endl;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- std::cout << " huli test :::: " << " tp_main_carrier->get_device_id() = " << -1 << std::endl;
|
|
|
- }
|
|
|
- if ( tp_avoid_catcher != NULL )
|
|
|
- {
|
|
|
- std::cout << " huli test :::: " << " tp_avoid_catcher->get_device_id() = " << tp_avoid_catcher->get_device_id() << std::endl;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- std::cout << " huli test :::: " << " tp_avoid_catcher->get_device_id() = " << -1 << std::endl;
|
|
|
- }
|
|
|
- std::cout << " huli test :::: " << " -------------------------- = " << 2 << std::endl;
|
|
|
+
|
|
|
+
|
|
|
+ std::cout << " huli test :::: " << " ----------------------------------------------------------------------------------------- = " << 2 << std::endl;
|
|
|
std::cout << " huli test :::: " << " m_dispatch_catcher_node.m_dispatch_control_status = " << m_dispatch_catcher_node.m_dispatch_control_status << std::endl;
|
|
|
if ( tp_main_catcher != NULL )
|
|
|
{
|
|
@@ -2025,7 +2090,7 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- std::cout << " huli test :::: " << " tp_main_catcher->get_device_id() = " << -1 << std::endl;
|
|
|
+ std::cout << " huli test :::: " << " tp_main_catcher->get_device_id() = " << -8888 << std::endl;
|
|
|
}
|
|
|
if ( tp_following_carrier != NULL )
|
|
|
{
|
|
@@ -2033,11 +2098,15 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- std::cout << " huli test :::: " << " tp_following_carrier->get_device_id() = " << -1 << std::endl;
|
|
|
+ std::cout << " huli test :::: " << " tp_following_carrier->get_device_id() = " << -8888 << std::endl;
|
|
|
}
|
|
|
|
|
|
- char ch;
|
|
|
- std::cin >> ch ;
|
|
|
+ std::cout << " huli test :::: " << " +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ = " << 3 << std::endl;
|
|
|
+ std::cout << " huli test :::: " << " m_parkspace_information_optimal.parkingspace_index_id = " << m_parkspace_information_optimal.parkingspace_index_id << std::endl;
|
|
|
+ std::cout << " huli test :::: " << " m_dispatch_destination = " << m_dispatch_destination << std::endl;
|
|
|
+
|
|
|
+// char ch;
|
|
|
+// std::cin >> ch ;
|
|
|
|
|
|
|
|
|
//结果汇总
|
|
@@ -2143,11 +2212,11 @@ Error_manager Dispatch_process::check_main_catcher(Dispatch_catcher_node & dispa
|
|
|
|
|
|
if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP )
|
|
|
{
|
|
|
- m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_PICKUP_START;
|
|
|
+ dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_PICKUP_START;
|
|
|
}
|
|
|
else if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE )
|
|
|
{
|
|
|
- m_dispatch_carrier_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_STORE_START;
|
|
|
+ dispatch_catcher_node.m_dispatch_control_status = Dispatch_process::Dispatch_control_status::DISPATCH_CATCHER_STORE_START;
|
|
|
}
|
|
|
return Error_code::SUCCESS;
|
|
|
}
|