501 lines
19 KiB
C++
501 lines
19 KiB
C++
#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();
|
|
}
|
|
|
|
|