HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit for v2"

Browse Source
This commit is contained in:
HiepLM
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

145
Controllers/Packages/nova5_control/CMakeLists.txt Normal file
View File

@@ -0,0 +1,145 @@
cmake_minimum_required(VERSION 3.0.2)
project(nova5_control)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++17)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
modbus_tcp
)
find_package(Boost REQUIRED COMPONENTS system)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES nova5_control
CATKIN_DEPENDS roscpp std_msgs modbus_tcp
DEPENDS Boost
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations ${GSTREAMER_LIBRARIES}
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_library(${PROJECT_NAME} src/nova5_modbus.cpp src/imr_nova_control.cpp)
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${GSTREAMER_LIBRARIES})
add_executable(test_nova example/simple.cpp)
add_dependencies(test_nova ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(test_nova ${catkin_LIBRARIES} nova5_control)
add_executable(test_thread_nova example/test_thread.cpp)
add_dependencies(test_thread_nova ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(test_thread_nova ${catkin_LIBRARIES} nova5_control)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/nova5_control.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/nova5_control_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_nova5_control.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

42
Controllers/Packages/nova5_control/README.md Normal file
View File

@@ -0,0 +1,42 @@
# Reference
https://docs.google.com/spreadsheets/d/1LkLH5IGXDXprNB6_w0-2-jNupRHEH-4PBC6LIrh3G1I/edit?gid=0#gid=0
https://www.dobot-robots.com/service/download-center?keyword=&products%5B%5D=535
https://tranduc-my.sharepoint.com/:w:/g/personal/tranduong1692_tranduc_onmicrosoft_com/EVSb3HeS-HZPlmAMflUsuLMBLrq5_Dnm5ku2Kj9Bu8C8Rw?e=FEROOU
# Require
Clone: http://git.pnkx/HiepLM/modbus.git
http://git.pnkx/HiepLM/libserial.git
# Guide
// Nên chạy trong một thread//
// Khởi tạo cổng kết nối robot thông qua modbus tcp //
cnt_nova5 *nova5 = nullptr;
nova5 = new cnt_nova5(IP,PORT);
//==================================================//
// Quy trình kết nối robot //
if((*nova5).nova_connect())
{
// Kiểm tra xem robot có đang trong trạng thái EMC không
if((*nova5).nova_robotMode() == 3) (*nova5).nova_powerOn();
// Sau khi kết nối thành công cần enable robot
if((*nova5).nova_robotMode() == 4) (*nova5).nova_enable();
// Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
if((*nova5).nova_robotMode() == 5) (*nova5).nova_startProgram();
}
else (*nova5).nova_close();
//==========================//
// Sử dụng hàm nova_movePoint() cần đưa vào vòng lặp chờ đến khi robot di chuyển xong và trả về phản hồi, sau đó mới được đưa ra lệnh điều khiển tiếp theo
// Ví dụ hàm nova_movePoint() //
if(count == 0)
{
// Set tốc độ global tay máy
nova5->nova_speedGlobal(50.0);
if(nova5->nova_movePoint(ORIGIN_COOR)==1 && count == 0) count ++;
}
//============================//

500
Controllers/Packages/nova5_control/example/simple.cpp Normal file
View File

@@ -0,0 +1,500 @@
#include"nova5_control/nova5_modbus.h"
#include <termios.h>
#include <stdio.h>
char getKey()
{
struct termios oldt, newt;
char ch;
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
ch = getchar();
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
return ch;
}
int main(int argc,char **argv)
{
ros::init(argc,argv,"EXAMPLE");
ros::NodeHandle nh("~");
std::string IP = "192.168.2.6";
int PORT = 502;
// Coordinates
std::vector<double> HOME = {-141.91,301.01,524.27,-179.4,-0.04,0.0};
std::vector<double> CENTER_P = {309.11,63.29,524.27,-179.4,-0.04,0.00};
std::vector<double> ORIGIN_COOR = {150.71,-435.9,524.27,-179.4,-0.04,-90.00};
std::vector<double> OK_COOR = {-141.91,301.01,150,-179.4,-0.04,0.0};
std::vector<double> NG_COOR = {-141.91,657.01,150,-179.4,-0.04,0.0};
std::vector<double> TEST_COOR = {250.71,-480.9,424.27,-179.4,-0.04,-90.00};
std::vector<double> STORE_COOR(6);
std::vector<double> Point(6);
std::vector<double> Point_current(6);
std::vector<double> Center_temp(6);
std::vector<double> Point_temp1(6);
std::vector<double> Point_temp2(6);
std::vector<double> Point_temp3(6);
uint16_t isCheck = 0;
int timeStamp_cam = 0;
static int count = 0, count_1 = 0;
static int center_count = 0;
static int get_point_current =0;
static int ob = 0;
static uint16_t speed = 0;
static int count_ok = 0;
static int count_ng = 0;
bool connect = false;
bool drag = false;
bool flag_home = false;
bool poweroff = false;
// Khởi tạo
std::shared_ptr<cnt_nova5> nova5_ptr_ = nullptr;
nova5_ptr_ = std::make_shared<cnt_nova5>(IP,PORT);
// Kết nối đến robot nova
connect = nova5_ptr_->nova_connect();
if(connect)
{
if(nova5_ptr_->nova_robotMode() == 3)
{
if(nova5_ptr_->nova_powerOn() == -1)
{
ROS_ERROR("Please check robot arm");
return -1;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi
}
}
// Sau khi kết nối thành công cần enable robot
nova5_ptr_->nova_enable();
// Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
nova5_ptr_->nova_startProgram();
}
else
{
// alarm = nova5_ptr_->nova_statusAlarm();
ROS_WARN("Robot is reconnecting with maximum time 15 minutes...");
for(int i=0; i<180U; ++i)
{
connect = nova5_ptr_->nova_connect();
if(connect || !ros::ok())
{
while(ros::ok())
{
double mode = nova5_ptr_->nova_robotMode();
if(nova5_ptr_->nova_statusAlarm() == 1 && mode == 9) ROS_ERROR("Kinematic ERROR! Please check robot!");
if(mode == 3)
{
if(nova5_ptr_->nova_powerOn() == -1)
{
ROS_ERROR("Please check robot arm");
return -1;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi
}
}
if(mode == 4) break;
ros::Duration(5).sleep();
}
// Sau khi kết nối thành công cần enable robot
nova5_ptr_->nova_enable();
// Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
nova5_ptr_->nova_startProgram();
break;
}
ros::Duration(5).sleep();
}
}
ros::Rate loop_rate(100);
//===========================Vòng lặp chính========================//
//=================================================================//
//=================================================================//
while(ros::ok() && connect)
{
static int temp = 0;
static bool checkconnect = false;
checkconnect = nova5_ptr_->nova_checkConnected();
if(checkconnect)
{
double robotmode = nova5_ptr_->nova_robotMode();
//===Kiểm tra trạng thái robot===//
//===============================//
if( robotmode == 3)
{
poweroff = true;
// Power on robot
if(nova5_ptr_->nova_powerOn() == 1)
{
// Sau khi power on cần enable robot
nova5_ptr_->nova_enable();
nova5_ptr_->nova_startProgram();
}
}
else if(robotmode == 6)
{
ROS_WARN("Robot is in drag mode");
drag = true;
}
if(drag == true && robotmode == 5)
{
ROS_WARN("PRESS: n to continute");
ROS_WARN("PRESS: h to go home");
char key = getKey();
ROS_WARN("%d", key);
/*Kiểm tra nếu key = n hoặc N thì thực hiện lệnh tiếp tục*/
if(key == 110 || key == 78)
{
nova5_ptr_->nova_startProgram();
drag = false;
get_point_current = 0;
count = 0;
center_count = 0;
flag_home = false;
}
/*Kiểm tra nếu key = h hoặc H thì thực hiện lệnh về home */
else if(key == 104)
{
nova5_ptr_->nova_startProgram();
drag = false;
get_point_current = 0;
center_count = 0;
ROS_INFO("Robot is comming home...");
flag_home = true;
}
}
//===============================//
//===============================//
//===Chu trình gắp phôi và lưu trữ phôi===//
//========================================//
/* Bước 0 ve home */
if(center_count == 0)
{
if(get_point_current == 0)
{
//Đọc tọa độ robot hiện tại trong vòng 6s
nova5_ptr_->nova_getCoorcurrent(Point_current);
ROS_ERROR("x current: %f", Point_current[0]);
ROS_ERROR("y current: %f", Point_current[1]);
ROS_ERROR("z current: %f", Point_current[2]);
if(CENTER_P[2] > (Point_current[2] + 10))
{
Center_temp[0] = Point_current[0];
Center_temp[1] = Point_current[1];
Center_temp[2] = nova5_ptr_->nova_check_z(Point_current[0],Point_current[1],CENTER_P[2],0);
Center_temp[3] = Point_current[3];
Center_temp[4] = Point_current[4];
Center_temp[5] = Point_current[5];
}
else if(CENTER_P[2] < (Point_current[2] - 10))
{
Center_temp[0] = CENTER_P[0];
Center_temp[1] = CENTER_P[1];
Center_temp[2] = nova5_ptr_->nova_check_z(CENTER_P[0],CENTER_P[1],Point_current[2], 0);
Center_temp[3] = CENTER_P[3];
Center_temp[4] = CENTER_P[4];
Center_temp[5] = CENTER_P[5];
}
else if(CENTER_P[2] >= (Point_current[2] - 10) && CENTER_P[2] < (Point_current[2] + 10)) center_count = 1;
ROS_WARN("x : %f", Center_temp[0]);
ROS_WARN("y : %f", Center_temp[1]);
ROS_WARN("z : %f", Center_temp[2]);
get_point_current = 1;
}
if(center_count == 0)
{
if(nova5_ptr_->nova_movePoint(Center_temp,100,1)==1 && center_count == 0) center_count++;
}
}
/* Bước 1 */
else if(center_count == 1)
{
if(nova5_ptr_->nova_movePoint(CENTER_P,100,1)==1 && center_count == 1)
{
center_count = 2;
}
}
if(flag_home && center_count == 2)
{
if(nova5_ptr_->nova_movePoint(HOME,100,1)==1 && center_count == 2)
{
ROS_INFO("Complete");
// nova5_ptr_->nova_stopProgram();
// nova5_ptr_->nova_close();
return 0;
}
}
/* Bước 2 di chuyển cánh tay đến vị trí chụp ảnh */
if(!flag_home && center_count == 2 && count == 0)
{
if(nova5_ptr_->nova_movePoint(ORIGIN_COOR,100,1)==1 && count == 0)
{
count++ ;
}
}
/* Bước 3 xác định vị trí vật thể và di chuyển tới vị trí trên vật thể */
else if(!flag_home && center_count == 2 && count == 1)
{
static bool continue_flag = false;
if(temp == 0)
{
// Đọc tọa độ vật trong thời gian max là 10s
while( ros::ok() && !nova5_ptr_->nova_statusAlarm())
{
if(!nova5_ptr_->nova_checkConnected() || !ros::ok() || nova5_ptr_->nova_statusAlarm() || nova5_ptr_->nova_robotMode() != 7)
{
continue_flag = true;
break;
}
ros::Duration(1).sleep();
ROS_INFO("Read camera");
if(nova5_ptr_->nova_readCoordinates(Point,ORIGIN_COOR,isCheck,timeStamp_cam) == 1)
{
// Lấy thời gian của máy tính hiện tại
ros::Time current_time = ros::Time::now();
// Chỉ lấy phần giây và ép kiểu sang int
int seconds = static_cast<int>(current_time.sec);
// Dữ liệu cũ không được cập nhật, chờ cho đến khi nhận được dữ liệu mới nhất
while ((seconds - timeStamp_cam) >= TIME_ERR && ros::ok())
{
ROS_WARN("Data is out of date!");
ROS_WARN("Wait for data update or press ctrl + c to cancel!");
// Lấy thời gian của máy tính hiện tại
ros::Time current_time = ros::Time::now();
// Chỉ lấy phần giây và ép kiểu sang int
int seconds = static_cast<int>(current_time.sec);
nova5_ptr_->nova_readCoordinates(Point,ORIGIN_COOR,isCheck,timeStamp_cam);
// Nếu khoảng thời giữa 2 lần gửi dữ liệu < 1s thì được coi là hợp lệ
if((seconds - timeStamp_cam) < TIME_ERR){
nova5_ptr_->nova_readCoordinates(Point,ORIGIN_COOR,isCheck,timeStamp_cam);
break;
}
if(!nova5_ptr_->nova_checkConnected() || nova5_ptr_->nova_robotMode() == 6)
{
ROS_WARN("Robot is in drag mode");
continue_flag = true;
break;
}
ros::Duration(1).sleep();
}
// Set tọa độ cho điểm trung gian 1
Point_temp1[0] = Point[0];
Point_temp1[1] = Point[1];
Point_temp1[2] = nova5_ptr_->nova_check_z(Point[0],Point[1],ORIGIN_COOR[2], 0);
Point_temp1[3] = Point[3];
Point_temp1[4] = Point[4];
Point_temp1[5] = Point[5];
temp = 1;
break;
}
}
if(continue_flag)
{
continue_flag = false;
continue;
}
// Point_temp1[0] = TEST_COOR[0];
// Point_temp1[1] = TEST_COOR[1];
// Point_temp1[2] = nova5_ptr_->nova_check_z(TEST_COOR[0],TEST_COOR[1],ORIGIN_COOR[2], 0);
// Point_temp1[3] = TEST_COOR[3];
// Point_temp1[4] = TEST_COOR[4];
// Point_temp1[5] = TEST_COOR[5];
temp =1;
}
if(count == 1 && continue_flag == false)
{
if(nova5_ptr_->nova_movePoint(Point_temp1,80,1) == 1) count++;
}
}
/* Bước 4 di chuyển tới vị trí vật thể */
else if(!flag_home && center_count == 2 && count == 2)
{
if(nova5_ptr_->nova_movePoint(Point,20,0)== 1 && count == 2)
{
nova5_ptr_->nova_onTool();
count++;
}
}
/* Bước 5 di chuyển tới vị trí trên vật thể */
else if(!flag_home && center_count == 2 && count == 3)
{
if(nova5_ptr_->nova_movePoint(Point_temp1,40,0)== 1 && count == 3)
{
count++;
}
}
/* Bước 6 di chuyển tới vị trí điểm home */
else if(!flag_home && center_count == 2 &&count == 4)
{
if(nova5_ptr_->nova_movePoint(CENTER_P,100,1)==1 && count == 4)
{
// Nếu vật thể là NG thì set điểm lưu trữ NG_COOR
if(isCheck == 0)
{
NG_COOR[0]+=55;
STORE_COOR[0] = NG_COOR[0];
STORE_COOR[1] = NG_COOR[1];
STORE_COOR[2] = NG_COOR[2];
STORE_COOR[3] = NG_COOR[3];
STORE_COOR[4] = NG_COOR[4];
STORE_COOR[5] = NG_COOR[5];
Point_temp3[0] = NG_COOR[0];
Point_temp3[1] = NG_COOR[1];
Point_temp3[2] = nova5_ptr_->nova_check_z(NG_COOR[0],NG_COOR[1],CENTER_P[2], 0);
Point_temp3[3] = NG_COOR[3];
Point_temp3[4] = NG_COOR[4];
Point_temp3[5] = NG_COOR[5];
speed = 30;
count_ng++;
}
// Nếu vật thể là OK thì set điểm lưu trữ OK_COOR
else if(isCheck == 1)
{
OK_COOR[0]+=55;
STORE_COOR[0] = OK_COOR[0];
STORE_COOR[1] = OK_COOR[1];
STORE_COOR[2] = OK_COOR[2];
STORE_COOR[3] = OK_COOR[3];
STORE_COOR[4] = OK_COOR[4];
STORE_COOR[5] = OK_COOR[5];
Point_temp3[0] = OK_COOR[0];
Point_temp3[1] = OK_COOR[1];
Point_temp3[2] = nova5_ptr_->nova_check_z(OK_COOR[0],OK_COOR[1],CENTER_P[2], 0);
Point_temp3[3] = OK_COOR[3];
Point_temp3[4] = OK_COOR[4];
Point_temp3[5] = OK_COOR[5];
speed = 20;
count_ok++;
}
count++;
}
}
/* Bước 7 di chuyển đến điểm trung gian trên điểm lưu trữ */
else if(!flag_home && center_count == 2 &&count == 5)
{
if(nova5_ptr_->nova_movePoint(Point_temp3,100,1) == 1 && count == 5)
{
count++ ;
}
}
/* Bước 8 di chuyển đến điểm lưu trữ */
else if(!flag_home && center_count == 2 &&count == 6)
{
if(nova5_ptr_->nova_movePoint(STORE_COOR,speed,0) == 1 && count == 6)
{
nova5_ptr_->nova_offTool(0.5);
count++ ;
}
}
/* Bước 9 di chuyển đến điểm trung gian trên điểm lưu trữ */
else if(!flag_home && center_count == 2 &&count == 7)
{
if(nova5_ptr_->nova_movePoint(Point_temp3,speed,0)==1 && count == 7)
{
count++;
}
}
/* Bước 10 reset các biến */
else if(!flag_home && center_count == 2 && count >= 8)
{
// timeStamp_cam_1 = timeStamp_cam;
temp = 0;
count = 0;
center_count = 1;
// if(isCheck > 1) isCheck =0;
if(count_ok > 4 && count_ng > 4) break;
// break;
}
//========================================//
//========================================//
// Hiển thị bước robot đang thực hiện
if((count + center_count) != count_1) ROS_INFO("STEP: %d", count + center_count);
count_1 = count + center_count;
}
else
{
// alarm = nova5_ptr_->nova_statusAlarm();
ROS_WARN("Robot is reconnecting with maximum time 15 minutes...");
for(int i=0; i<180U; ++i)
{
if(!ros::ok()) break;
if(nova5_ptr_->nova_connect() )
{
nova5_ptr_->nova_stopProgram();
// Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
nova5_ptr_->nova_startProgram();
temp = 0;
count = 0;
center_count = 0;
get_point_current = 0;
flag_home = false;
break;
}
ros::Duration(5).sleep();
}
}
loop_rate.sleep();
ros::spinOnce();
}
// nova5_ptr_->nova_stopProgram();
// nova5_ptr_->nova_close();
}

101
Controllers/Packages/nova5_control/example/test_thread.cpp Normal file
View File

@@ -0,0 +1,101 @@
#include "nova5_control/imr_nova_control.h"
#include <iostream>
#include <memory>
#include <ros/ros.h>
#include <std_msgs/UInt16.h>
std::shared_ptr<imr_nova_control> controller = nullptr;
unsigned int ok_count_max = 0;
unsigned int ng_count_max = 1;
bool enable = true;
bool continue_flag = false;
bool go_home_flag = false;
bool power_on_flag = false;
void callBack(const std_msgs::UInt16::ConstPtr& msg)
{
if(controller != nullptr)
{
switch (msg->data)
{
case 1:
controller->startHomeThread();
// continue_flag = true;
// go_home_flag = false;
// power_on_flag = false;
break;
case 2:
// continue_flag = false;
// go_home_flag = true;
// power_on_flag = false;
break;
case 3:
// continue_flag = false;
// go_home_flag = false;
// power_on_flag = true;
break;
default:
// continue_flag = false;
// go_home_flag = false;
// power_on_flag = false;
break;
}
}
}
int main(int argc, char** argv) {
// Initialize the ROS node
ros::init(argc, argv, "test_imr_nova_control");
ros::NodeHandle nh = ros::NodeHandle("~");
ros::Subscriber sub = nh.subscribe("mode", 1, callBack);
ros::Publisher pub = nh.advertise<std_msgs::UInt16>("status", 1);
try {
// Initialize ROS time
// ros::Time::init();
// Create an instance of imr_nova_control
controller = std::make_shared<imr_nova_control>();
controller->ok_count_max_ = &ok_count_max;
controller->ng_count_max_ = &ng_count_max;
controller->enable_ = &enable;
controller->continue_flag_ = &continue_flag;
controller->go_home_flag_ = &go_home_flag;
controller->power_on_flag_ = &power_on_flag;
#if 0
controller->startHomeThread();
#endif
#if 0
controller->startModeThread();
#endif
std::cout << "imr_nova_control instance created successfully." << std::endl;
// Run ROS spin to process callbacks (if applicable)
// ros::AsyncSpinner spinner(2); // Adjust the number of threads as needed
// spinner.start();
ros::Rate rate(5);
while (ros::ok())
{
std_msgs::UInt16 msg;
msg.data = controller->statusCode_;
pub.publish(msg);
ros::spinOnce();
rate.sleep();
}
} catch (const std::exception& e) {
std::cerr << "An error occurred: " << e.what() << std::endl;
}
ros::spin();
return 0;
}

95
Controllers/Packages/nova5_control/include/nova5_control/imr_nova_control.h Normal file
View File

@@ -0,0 +1,95 @@
#ifndef __NOVA5_CONTROL_IMR_NOVA_CONTROL_H__
#define __NOVA5_CONTROL_IMR_NOVA_CONTROL_H__
// #include <ros/ros.h>
#include <termios.h>
#include <stdio.h>
#include <thread>
#include"nova5_control/nova5_modbus.h"
class imr_nova_control
{
public:
imr_nova_control();
virtual ~imr_nova_control();
void startModeThread();
void stopModeThread();
void startHomeThread();
void stopHomeThread();
unsigned int *ok_count_max_;
unsigned int *ng_count_max_;
bool *enable_;
double mode_;
bool *continue_flag_;
bool *go_home_flag_;
bool *power_on_flag_;
enum statusCode
{
ROBOT_WAITING = 98,
ROBOT_RECONNECTING = 99,
ROBOT_RUNNING = 100,
ROBOT_DISCONNECTED = 101,
ROBOT_READ_DATA_ZERO = 102,
ROBOT_DATA_NOT_UPDATED = 103,
ROBOT_TARGET_COORDINATES_OUT_OF_RANGE = 104,
ROBOT_COMING_HOME = 105,
ROBOT_CHECK_EMERGENCY_STOP_BUTTON = 106,
ROBOT_IN_ALARM_STATE = 107,
ROBOT_POWER_ON_WAITTING = 108,
ROBOT_POWER_ON = 109,
ROBOT_DRAG = 110,
ROBOT_WAITING_SELECT_CONTINUE_OR_GO_HOME = 111,
ROBOT_OK_FULL = 112,
ROBOT_NG_FULL = 113,
ROBOT_ERROR = 999
};
/**
* @var statusCode_
* 98: Robot is waiting
* 99: Robot is reconnecting
* 100: Robot is running
* 101: Robot is disconnected
* 102: Read data is 0 (Camera can not detect object or lost connection)
* 103: Data is not updated (camera maybe disconnected)
* 104: Target coordinates are not within the working range
* 105: Robot is comming home
* 106: Check the robot's emergency stop button
* 107: Robot is in alarm state
* 108: Robot is power on
* 109: Robot is in drag mode
* 110: OK object is over the limit
* 111: NG object is over the limit
* 999: Robot is error
*/
statusCode statusCode_;
private:
char getKey();
void imr_dobot_control_thread();
void imr_dobot_go_home_thread();
// Coordinates
std::vector<double> HOME = {-141.91,301.01,524.27,-179.4,-0.04,0.0};
std::vector<double> CENTER_P = {309.11,63.29,524.27,-179.4,-0.04,0.00};
std::vector<double> ORIGIN_COOR = {150.71,-435.9,524.27,-179.4,-0.04,-90.00};
std::vector<double> OK_COOR = {-141.91,301.01,150,-179.4,-0.04,0.0};
std::vector<double> NG_COOR = {-141.91,657.01,150,-179.4,-0.04,0.0};
std::string IP = "192.168.2.6";
int PORT = 502;
bool modeThreadRunning_; // Variable to check if the thread is running
bool homeThreadRunning_; // Variable to check if the thread is running
std::shared_ptr<std::thread> thr_main_;
std::shared_ptr<std::thread> thr_go_home_;
};
#endif // __NOVA5_CONTROL_IMR_NOVA_CONTROL_H__

300
Controllers/Packages/nova5_control/include/nova5_control/nova5_modbus.h Normal file
View File

@@ -0,0 +1,300 @@
// nova5_modbus.h
#ifndef NOVA5_CONTROL_H
#define NOVA5_CONTROL_H
// #pragma once
#include<ros/ros.h>
#include <boost/shared_ptr.hpp>
#include<iostream>
#include<math.h>
#include<cstring>
#include<array>
#include<stdexcept>
#include"modbus_tcp/modbus.h"
#define TIME_ERR 1
#define ERR -1
#define TIME_OUT 1.5
// Coil Registers
#define COIL_START 0
#define COIL_PAUSE 1
#define COIL_CONTINUE 2
#define COIL_STOP 3
#define COIL_EMERGENCY_STOP 4
#define COIL_CLEAR_ALARM 5
#define COIL_POWER_ON 6
#define COIL_ENABLE 100
#define COIL_DISABLE 101
// #define USER_DEFINED_COIL_START 3095
#define COILD_MOVE 3095
#define COILD_MOVING_DONE 3096
#define COILD_TOOL 3097
#define COILD_MOVE_MODE 3098 // 0 moveJ - 1 moveL
// #define USER_DEFINED_COIL_END 9999
// Discrete Input Registers
#define DISCRETE_STOP_STATE 1
#define DISCRETE_PAUSE_STATE 2
#define DISCRETE_RUNNING_STATE 3
#define DISCRETE_ALARM_STATE 4
#define DISCRETE_RESERVED_1 5
#define DISCRETE_REMOTE_CONTROL 7
// Input Registers
#define INPUT_ROBOT_MODE_START 1012
#define INPUT_ROBOT_MODE_END 1015
#define INPUT_TIMESTAMP_START 1016
#define INPUT_TIMESTAMP_END 1019
#define INPUT_TARGET_JOINT_POSITION_START 1096
#define INPUT_TARGET_JOINT_POSITION_END 1119
#define INPUT_TARGET_JOINT_CURRENT_START 1168
#define INPUT_TARGET_JOINT_CURRENT_END 1191
#define INPUT_ACTUAL_JOINT_POSITION_START 1216
#define INPUT_ACTUAL_JOINT_POSITION_END 1239
#define INPUT_ACTUAL_JOINT_CURRENT_START 1264
#define INPUT_ACTUAL_JOINT_CURRENT_END 1287
#define INPUT_ROBOT_BRAKE_STATUS_H 1512 //HIGH BYTE
#define INPUT_ROBOT_ENABLING_STATUS_L 1513 //LOW BYTE
#define INPUT_ROBOT_DRAG_STATUS_H 1513 //HIGH BYTE
#define INPUT_ROBOT_DRAG_STATUS_L 1514 //HIGH BYTE
#define INPUT_ROBOT_ALARM_STATUS_H 1514 //HIGH BYTE
#define INPUT_LOAD_WEIGHT_START 1584
#define INPUT_LOAD_WEIGHT_END 1587
#define INPUT_ECCENTRIC_DISTANCE_X_START 1588
#define INPUT_ECCENTRIC_DISTANCE_X_END 1591
#define INPUT_ECCENTRIC_DISTANCE_Y_START 1592
#define INPUT_ECCENTRIC_DISTANCE_Y_END 1595
#define INPUT_ECCENTRIC_DISTANCE_Z_START 1596
#define INPUT_ECCENTRIC_DISTANCE_Z_END 1599
#define INPUT_USER_COORDINATES_START 1600
#define INPUT_USER_COORDINATES_END 1623
#define INPUT_TOOL_COORDINATES_START 1624
#define INPUT_TOOL_COORDINATES_END 1647
// Holding Registers
#define HOLDING_GLOBAL_SPEED 100
// #define HOLDING_USER_DEFINED_START 3095
#define HOLDING_TARGET_POINT_START 3095
#define HOLDING_TARGET_POINT_END 3118
#define HOLDING_CAMERA_POINT_START 3119
#define HOLDING_CAMERA_POINT_END 3134
#define HOLDING_CAMERA_OK 3135
#define HOLDING_CAMERA_TIMESTAMP_START 3136
#define HOLDING_CAMERA_TIMESTAMP_END 3139
#define HOLDING_POINT_CURRENT_START 3140
#define HOLDING_POINT_CURRENT_END 3163
#define HOLDING_Z_MIN_USER_SET_START 3164
#define HOLDING_Z_MIN_USER_SET_END 3167
// #define HOLDING_USER_DEFINED_END 8999
// #define HOLDING_CURRENT_SCREW_INDEX 9000
#define HOLDING_ALARM_CODE 9054
class cnt_nova5
{
public:
cnt_nova5(std::string ip_address, int port);
virtual ~cnt_nova5();
/**
* @brief Connect to nova
*/
int nova_connect(void);
/**
* @brief Check connection
*/
bool nova_checkConnected(void);
void nova_disable(void);
/**
* @brief Close to nova
*/
void nova_close(void);
/**
* @brief Alarm notification
*/
bool nova_statusAlarm(void);
/**
* @brief Clear alarm
*/
void nova_clearAlarm(void);
/**
* @brief Power on robot
*/
int nova_powerOn(void);
/**
* @brief enable robot
*/
bool nova_enable(void);
/**
* @brief start program
*/
bool nova_startProgram(void);
/**
* @brief stop program
*/
void nova_stopProgram(void);
/**
* @brief Read robot mode and return robot mode
3: Emergency stop mode, not powered on
4: Powered on mode, not enabled
5: Enabled mode, program not started
6: Drag mode
7: Program started mode
9: Kinematic error of robot nova
*/
double nova_robotMode(void);
/**
* @brief Select moveJ or moveL
*/
void nova_robotMove(void);
/**
* @brief Set speed global for nova
* @param speed Speed setting for nova
*/
void nova_speedGlobal(uint16_t speed);
/**
* @brief Check nova status (nova has 7 status registers form 1-7)
* @param add Address of status register
* @param value Register status
*/
int nova_status(int add, bool &value);
/**
* @brief Check nova multistate (nova has 7 status registers form 1-7)
* @param add First status register address
* @param length Number of rgisters to read
* @param buff Register states
*/
int nova_multistate(int add, int length, bool *buff);
/**
* @brief Get 6 values ​​x, y, z, Rx, Ry, Rz
* @param Point Condinates after conversion
* @param Point_initial Scan coordinates
* @param isCheck True or false check variable
* @param Timestamp Time stamp
*/
int nova_readCoordinates(std::vector<double>& Point,std::vector<double>& Point_initial, uint16_t& isCheck, int& Timestamp);
/**
* @brief Get coordinates from camera
* @param Point Variable store coordinates current
*/
int nova_getCoorcurrent(std::vector<double>& Point);
/**
* @brief Read time stamp nova
* @param time Time stamp of nova
*/
int nova_timeStamp(uint64_t& time);
/**
* @brief
*/
int nova_createArea(void);
/**
* @brief Turn on tool
*/
void nova_onTool(void);
/**
* @brief Turn off tool
*/
void nova_offTool(const double delay);
/**
* @brief Move robot nova
* @param Point Target coordinates
* @param speed cai toc do cho tay may
* @param mode 0 - moveL, const int mode = 1
* 1 - moveJ
*/
int nova_movePoint(std::vector<double>& Point,const int speed, const int mode = 1);
/**
* @brief Check z
*
* @param x
* @param y
* @param z
* @param choose 0 - z min theo mac dinh 200 mm
* 1 - z min theo tham so duoc set tu CCBOX
*/
double nova_check_z(double& x, double& y, double& z, int choose = 0);
int movePointRunning_;
bool *run_ptr_;
private:
/**
* @brief Robot move done notification
*/
bool nova_movingDone(void);
/**
* @brief Allow the robot to start moving
*/
void nova_allowMove(void);
/**
* @brief Dismiss the robot moving
*/
void nova_dismissMove(void);
/**
* @brief Send 6 values ​​x, y, z, Rx, Ry, Rz
* @param mode 0 - moveJ, const int mode = 0
* 1 - moveL
* @param Point Coordinates to sent
*/
int nova_sendCoordinates(std::vector<double>& Point);
/**
* @brief Check point coordinates
* return 1: Point within range
* return 0: Point is not range
* return -1: Error
* @param x
* @param y
* @param z
*/
int workArealimit(double x, double y, double z);
/**
* @brief Convert object coordinates to robot tool condinates
* @param Point Condinates after conversion
* @param Point_initial Scan coordinates
* @param Coor Object coordinates
*/
int convert(std::vector<double>& Point,std::vector<double>& Point_initial, std::vector<double>& Coor);
/**
* @brief Get z min and return z min
*/
double getZmin(void);
/* data */
enum WorkAreaStatus {
WITHIN_LIMIT = 1,
OUT_OF_LIMIT = 0,
ERROR = -1
};
boost::shared_ptr<modbus> nova_;
bool inter_var;
bool send;
double a1,b1,c1;
double a2,b2;
double r_max;
double r_min;
double z_min_static;
// double safety_factor;
double x_deviation;
double y_deviation;
double z_deviation;
bool flag_move;
uint64_t mode_store;
};
#endif // NOVA5_CONTROL_H

68
Controllers/Packages/nova5_control/package.xml Normal file
View File

@@ -0,0 +1,68 @@
<?xml version="1.0"?>
<package format="2">
<name>nova5_control</name>
<version>0.0.0</version>
<description>The nova5_control package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="robotics@todo.todo">robotics</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/nova5_control</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<build_depend>modbus_tcp</build_depend>
<build_export_depend>modbus_tcp</build_export_depend>
<exec_depend>modbus_tcp</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

1331
Controllers/Packages/nova5_control/src/imr_nova_control.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

670
Controllers/Packages/nova5_control/src/nova5_modbus.cpp Normal file
View File

@@ -0,0 +1,670 @@
#include"nova5_control/nova5_modbus.h"
cnt_nova5::cnt_nova5(std::string ip_address, int port)
: nova_(nullptr),
movePointRunning_(0),
run_ptr_(nullptr)
{
nova_ = boost::make_shared<modbus>(ip_address, port);
inter_var = false;
// Declare the values of the sphere O
a1 = 0.0;
b1 = 0.0;
c1 = 0.0;
r_max = 750.0;
// Declare the values of cylinder I
a2 = 0.0;
b2 = 0.0;
r_min = 250.0;
// Deviation of camera
x_deviation = 8 + (350/2.0);
y_deviation = 100.0 + (234.5/2.0);
z_deviation = 140.5 + 5 - 28.0 + 3.5; // Chiều cao tool + chiều cao camera + dung sai
z_min_static = 170;
mode_store = 0;
flag_move = 0;
}
cnt_nova5::~cnt_nova5()
{
nova_stopProgram();
nova_close();
}
int cnt_nova5::nova_connect(void)
{
ROS_INFO("Nova is connecting.......");
try
{
if(!nova_->modbus_connect())
{
ROS_ERROR("Nova connection is error");
return 0;
}
else
{
ROS_INFO("Nova is Connected");
return 1;
}
}
catch (...)
{
ROS_ERROR_STREAM("Caught unknown exception");
return -1;
}
}
bool cnt_nova5::nova_checkConnected(void)
{
try
{
return nova_->_connected;
}
catch (const std::exception& e)
{
ROS_ERROR_STREAM(e.what());
}
catch (...)
{
ROS_ERROR_STREAM("Caught unknown exception");
}
}
void cnt_nova5::nova_disable(void)
{
nova_dismissMove();
nova_stopProgram();
nova_->modbus_write_coil(COIL_DISABLE, true);
ros::Duration(0.1).sleep();
nova_->modbus_write_coil(COIL_DISABLE, false);
ros::Duration(0.6).sleep();
}
void cnt_nova5::nova_close(void)
{
ROS_INFO("Close robot");
nova_disable();
nova_->modbus_close();
}
int cnt_nova5::workArealimit(double x, double y, double z)
{
double dO_instant = z + (1/(2*r_max)) * (pow((x + a1), 2) + pow((y + b1), 2));
// INNER BOUNDING CYLINDER
double dI_instant = sqrt(pow((x + a2),2) + pow((y + b2),2));
if(dO_instant < r_max)
{
return((dI_instant > r_min) ? WITHIN_LIMIT : OUT_OF_LIMIT);
}
else return ERROR;
}
int cnt_nova5::nova_sendCoordinates(std::vector<double>& Point)
{
try
{
if(Point.size() != 6U) throw std::invalid_argument("A point has 6 parameters");
if(workArealimit(Point[0],Point[1],Point[2]) == 0) throw std::out_of_range("Point beyond work area");
if(workArealimit(Point[0],Point[1],Point[2]) == ERROR) throw std::runtime_error("Unknown error");
std::array<uint16_t,24> part;
std::vector<double> Point_send(6);
if(Point[2] < z_min_static)
{
Point_send[0] = Point[0];
Point_send[1] = Point[1];
Point_send[2] = z_min_static;
Point_send[3] = Point[3];
Point_send[4] = Point[4];
Point_send[5] = Point[5];
}
else
{
Point_send[0] = Point[0];
Point_send[1] = Point[1];
Point_send[2] = Point[2];
Point_send[3] = Point[3];
Point_send[4] = Point[4];
Point_send[5] = Point[5];
}
for (int i = 0; i < 6U; ++i)
{
std::memcpy(&part[i*4], reinterpret_cast<uint8_t*>(&Point_send[i]), sizeof(double));
}
if(nova_->modbus_write_registers(HOLDING_TARGET_POINT_START,24,part.data()) == 0) return 1;
else return 0;
}
catch(const std::invalid_argument& e)
{
ROS_ERROR_STREAM("ERROR: " << e.what());
ros::Duration(TIME_ERR).sleep();
return ERR;
}
catch(const std::out_of_range& e)
{
ROS_ERROR_STREAM("ERROR: " << e.what());
ros::Duration(TIME_ERR).sleep();
return ERR;
}
catch(const std::runtime_error& e)
{
ROS_ERROR_STREAM("ERROR send: " << e.what());
ros::Duration(TIME_ERR).sleep();
return ERR;
}
}
int cnt_nova5::convert(std::vector<double>& Point, std::vector<double>& Point_initial, std::vector<double>& Coor)
{
if(Point.size() == 6U)
{
for(int i = 0; i < Point.size(); ++i)
{
Point[i] = Point_initial[i];
}
Point[0] = Point[0] + x_deviation - Coor[1] - (x_deviation - Coor[1])*0.15; // x
Point[1] = Point[1] - y_deviation + Coor[2]; //y
Point[2] = Point[2] + z_deviation - Coor[0]; // z
Point[3] = Point[3]; // Rx
Point[4] = Point[4]; // Ry
Point[5] = Point[5] + 90.0 - Coor[3]; // Rz
return 1;
}
else return ERR;
}
int cnt_nova5::nova_readCoordinates(std::vector<double>& Point,std::vector<double>& Point_initial, uint16_t& isCheck, int& Timestamp)
{
try
{
std::array<uint16_t,21> register_buffer;
std::vector<double> Point_receive(4);
if(Point.size() != 6U) throw std::invalid_argument("A point has 6 parameters");
if (register_buffer.size() < 21) throw std::runtime_error("Buffer size is too small");
int status = nova_->modbus_read_holding_registers(HOLDING_CAMERA_POINT_START,21,register_buffer.data());
if (status == 0 )
{
// ROS_INFO("Successfully read registers coordinates");
for (int i = 0; i < 4U; ++i)
{
memcpy(&Point_receive[i], &register_buffer[i * 4U], sizeof(double));
// ROS_INFO("Double value %d: %f", i, Point_receive[i]);
}
isCheck = register_buffer[16];
Timestamp = (static_cast<int>(register_buffer[17]) << 16) | register_buffer[18];
// convert(Point,Point_initial,Point_receive);
if(Point_receive[1] == 0 && Point_receive[2] == 0 && isCheck == 0) throw std::runtime_error("Camera error");
if(convert(Point,Point_initial,Point_receive) == -1) throw std::invalid_argument("A point has 6 parameters");
if(workArealimit(Point[0],Point[1],Point[2]) == 0) throw std::out_of_range("Point beyond work area");
if(workArealimit(Point[0],Point[1],Point[2]) == -1) throw std::runtime_error("Unknown error");
return 1;
}
else
{
// ROS_ERROR("Failed to read registers coordinates");
return 0;
}
}
catch(const std::invalid_argument& e)
{
ROS_ERROR_STREAM("ERROR: " << e.what());
ros::Duration(TIME_ERR).sleep();
return ERR;
}
catch(const std::out_of_range& e)
{
ROS_ERROR_STREAM("ERROR: " << e.what());
ros::Duration(TIME_ERR).sleep();
return ERR;
}
catch(const std::runtime_error& e)
{
ROS_ERROR_STREAM("ERROR: " << e.what());
ros::Duration(TIME_ERR).sleep();
return ERR;
}
}
int cnt_nova5::nova_status(int add, bool &value)
{
try
{
if(add < DISCRETE_STOP_STATE || add > DISCRETE_REMOTE_CONTROL) throw std::out_of_range("Register does not exist");
int status = nova_->modbus_read_coil(add, value);
if(status == 0)
{
// ROS_INFO("Successfully read register status");
return 1;
}
else
{
// ROS_ERROR("Failed to read register status %d", status);
return 0;
}
}
catch(const std::out_of_range& e)
{
ROS_ERROR_STREAM("ERROR: " << e.what());
ros::Duration(TIME_ERR).sleep();
return ERR;
}
catch(...)
{
ROS_ERROR_STREAM("Caught unknown exception");
return ERR;
}
}
int cnt_nova5::nova_multistate(int add, int length, bool *buff)
{
try
{
if(add < DISCRETE_STOP_STATE || add > DISCRETE_REMOTE_CONTROL) throw std::out_of_range("Register does not exist");
if(length > (DISCRETE_REMOTE_CONTROL - add) || length <= 0) throw std::out_of_range("Invalid length value");
int status = nova_->modbus_read_coils(add, length, buff);
if(status == 0)
{
// ROS_INFO("Successfully read register multistate");
return 1;
}
else
{
// ROS_ERROR("Failed to read register multistate %d", status);
return 0;
}
}
catch(const std::out_of_range& e)
{
ROS_ERROR_STREAM("ERROR: " << e.what());
ros::Duration(TIME_ERR).sleep();
return ERR;
}
catch(...)
{
ROS_ERROR_STREAM("Caught unknown exception");
return ERR;
}
}
int cnt_nova5::nova_timeStamp(uint64_t& time)
{
try
{
std::array<uint16_t,4> register_buffer;
int status = nova_->modbus_read_holding_registers(INPUT_TIMESTAMP_START,4, register_buffer.data());
if(status == 0)
{
// ROS_INFO("Successfully read register time stamp");
memcpy(&time,&register_buffer,sizeof(uint64_t));
return 1;
}
else
{
// ROS_ERROR("Failed to read register time stamp %d", status);
return 0;
}
}
catch(const std::exception& e)
{
ROS_ERROR_STREAM("Caught unknown exception");
return ERR;
}
}
double cnt_nova5::nova_robotMode(void)
{
uint64_t mode_uint64;
std::array<uint16_t,4> register_buffer;
int status = nova_->modbus_read_input_registers(INPUT_ROBOT_MODE_START,4,register_buffer.data());
if(status == 0)
{
// ROS_INFO("Successfully read register robot mode");
memcpy(&mode_uint64, &register_buffer, sizeof(uint64_t));
if(mode_uint64 != mode_store) ROS_INFO("Mode: %ld",mode_uint64);
mode_store = mode_uint64;
double mode_double = (double)mode_uint64;
return mode_uint64;
}
else
{
// ROS_ERROR("Failed to read register robot mode %d", status);
return ERR;
}
}
int cnt_nova5::nova_powerOn(void)
{
static int count = 0;
if(run_ptr_ == nullptr) run_ptr_ = &inter_var;
while(ros::ok() && nova_robotMode() == 3 && *run_ptr_)
{
ROS_INFO("Wait 25s for robot to power on...");
if(nova_statusAlarm() == 1)
{
nova_->modbus_write_coil(COIL_CLEAR_ALARM, true);
ros::Duration(0.01).sleep();
}
nova_->modbus_write_coil(COIL_POWER_ON, true);
ros::Duration(20).sleep();
if(nova_robotMode() == 3)
{
nova_->modbus_write_coil(COIL_CLEAR_ALARM, false);
ros::Duration(0.01).sleep();
nova_->modbus_write_coil(COIL_POWER_ON, false);
ros::Duration(0.01).sleep();
count++;
if(count>1)
{
ROS_ERROR("Power on failed...");
count = 0;
return -1;
break;
}
}
else
{
ROS_INFO("Power on successfully");
nova_->modbus_write_coil(COIL_CLEAR_ALARM, false);
ros::Duration(0.01).sleep();
nova_->modbus_write_coil(COIL_POWER_ON, false);
ros::Duration(0.01).sleep();
return 1;
break;
}
ros::spinOnce();
}
return 0;
}
bool cnt_nova5::nova_enable(void)
{
int status_clear_alarm = nova_->modbus_write_coil(COIL_CLEAR_ALARM, true);
ros::Duration(0.01).sleep();
int status_enable = nova_->modbus_write_coil(COIL_ENABLE, true);
ros::Duration(0.6).sleep();
nova_->modbus_write_coil(COIL_CLEAR_ALARM, false);
ros::Duration(0.01).sleep();
nova_->modbus_write_coil(COIL_ENABLE, false);
ros::Duration(0.01).sleep();
double mode = nova_robotMode();
if(mode >= 5 && status_clear_alarm == 0 && status_enable == 0 )
{
ROS_INFO("Enable done");
return true;
}
else
{
ROS_ERROR("Enable failed");
return false;
}
}
void cnt_nova5::nova_allowMove(void)
{
nova_->modbus_write_coil(COILD_MOVE,true);
ros::Duration(0.3).sleep();
}
void cnt_nova5::nova_dismissMove(void)
{
nova_->modbus_write_coil(COILD_MOVE,false);
ros::Duration(0.01).sleep();
}
void cnt_nova5::nova_stopProgram(void)
{
nova_->modbus_write_coil(COIL_STOP, true);
ros::Duration(0.01).sleep();
nova_->modbus_write_coil(COIL_STOP, false);
ros::Duration(0.5).sleep();
}
bool cnt_nova5::nova_startProgram(void)
{
flag_move = 0;
nova_dismissMove();
nova_offTool(0.01);
if(run_ptr_ == nullptr) run_ptr_ = &inter_var;
int i = 0;
while(i < 4 && ros::ok() && *run_ptr_)
{
nova_->modbus_write_coil(COIL_START, true);
ros::Duration(0.3).sleep();
nova_->modbus_write_coil(COIL_START, false);
ros::Duration(0.02).sleep();
uint64_t mode = nova_robotMode();
if(mode == 7) break;
i++;
}
uint64_t mode = nova_robotMode();
if(mode == 7) return true;
else return false;
}
bool cnt_nova5::nova_movingDone(void)
{
bool status;
nova_->modbus_read_coil(COILD_MOVING_DONE, status);
// ROS_INFO("STATUS movingDone %d",status);
return status;
}
bool cnt_nova5::nova_statusAlarm(void)
{
bool alarm = false;
nova_->modbus_read_input_bit(DISCRETE_ALARM_STATE,alarm);
// ROS_INFO("ALARM ST: %x",alarm);
return alarm;
}
void cnt_nova5::nova_clearAlarm()
{
nova_->modbus_write_coil(COIL_CLEAR_ALARM, true);
ros::Duration(0.1).sleep();
nova_->modbus_write_coil(COIL_CLEAR_ALARM, false);
ros::Duration(0.01).sleep();
}
void cnt_nova5::nova_onTool(void)
{
ROS_INFO("ON TOOL");
nova_->modbus_write_coil(COILD_TOOL, true);
ros::Duration(2).sleep();
}
void cnt_nova5::nova_offTool(const double delay)
{
ROS_INFO("OFF TOOL");
nova_->modbus_write_coil(COILD_TOOL, false);
ros::Duration(delay).sleep();
}
void cnt_nova5::nova_speedGlobal(uint16_t speed)
{
nova_->modbus_write_register(HOLDING_GLOBAL_SPEED,speed);
// ros::Duration(0.01).sleep();
}
int cnt_nova5::nova_getCoorcurrent(std::vector<double>& Point)
{
try
{
if(Point.size() != 6U) throw std::invalid_argument("Coordinates have 6 parameters");
std::array<uint16_t,24> register_buffer;
for(int i = 0; i <= 5; i++)
{
ros::Duration(1).sleep();
int status = nova_->modbus_read_holding_registers(HOLDING_POINT_CURRENT_START,24,register_buffer.data());
if (status == 0)
{
for(int i = 0; i < 6U; ++i)
{
memcpy(&Point[i], &register_buffer[i * 4U], sizeof(double));
}
if((Point[0] != 0 && Point[1] != 0 && Point[2] !=0) ||
(!ros::ok()) || nova_robotMode() != 7 || !nova_checkConnected() && *run_ptr_ == false) break;
if(i == 5) return 0;
}
else return ERR;
}
return 0;
}
catch(const std::invalid_argument& e)
{
ROS_ERROR_STREAM("ERROR: " << e.what());
ros::Duration(TIME_ERR).sleep();
return ERR;
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
return ERR;
}
}
double cnt_nova5::getZmin(void)
{
std::array<uint16_t,4> register_buffer;
double Z_min;
nova_->modbus_read_holding_registers(HOLDING_Z_MIN_USER_SET_START,4,register_buffer.data());
memcpy(&Z_min, &register_buffer, sizeof(double));
ROS_INFO("Z min : %f",Z_min);
return Z_min;
}
double cnt_nova5::nova_check_z(double& x, double& y, double& z, int choose)
{
static double z_min_store = getZmin();
static double z_min;
double z_max = r_max - 10 - (1/(2*r_max)) * (pow((x + a1), 2) + pow((y + b1), 2));
if(choose == 1) z_min = z_min_static;
else if(choose == 0) z_min = z_min_store;
if(z_max > z_min)
{
if(z > z_min && z < z_max) return z;
else if(z <= z_min) return z_min;
else if(z >= z_max) return z_max;
return ERR;
}
else return z_min;
}
int cnt_nova5::nova_movePoint(std::vector<double>& Point,const int speed, const int mode)
{
try
{
/* code */
if(Point.size() != 6U) throw std::invalid_argument("A point has 6 parameters - move Point");
if(workArealimit(Point[0],Point[1],Point[2]) == 0) throw std::out_of_range("Point beyond work area - move Point");
if(workArealimit(Point[0],Point[1],Point[2]) == -1) throw std::runtime_error("Unknown error - move Point");
movePointRunning_ = 1;
if(flag_move == 0)
{
// Chọn chế độ di chuyển moveJ move L
nova_->modbus_write_coil(COILD_MOVE_MODE, mode);
// Cài đặt tốc độ toàn cục cho tay máy
nova_speedGlobal(speed);
// Gửi tọa độ mục tiêu và xác nhận cho robot di chuyển đến tọa độ mục tiêu cho robot tay máy
if(nova_sendCoordinates(Point) == 1) nova_allowMove();
else return 0;
flag_move = 1;
}
// Chờ cho robot di chuyển xong
if(nova_movingDone() == 1)
{
nova_dismissMove();
flag_move = 0;
return 1;
}
// If no movement completed, return an error
return 0;
}
catch(const std::invalid_argument& e)
{
ROS_ERROR_STREAM("ERROR: " << e.what());
ros::Duration(TIME_ERR).sleep();
movePointRunning_ = ERR;
return ERR;
}
catch(const std::out_of_range& e)
{
ROS_ERROR_STREAM("ERROR: " << e.what());
ros::Duration(TIME_ERR).sleep();
movePointRunning_ = ERR;
return ERR;
}
catch(const std::runtime_error& e)
{
ROS_ERROR_STREAM("ERROR send: " << e.what());
ros::Duration(TIME_ERR).sleep();
movePointRunning_ = ERR;
return ERR;
}
catch(const std::exception& e)
{
ROS_ERROR_STREAM("ERROR: " << e.what());
ros::Duration(TIME_ERR).sleep();
movePointRunning_ = ERR;
return ERR;
}
}