#include"nova5_control/nova5_modbus.h" #include #include 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 HOME = {-141.91,301.01,524.27,-179.4,-0.04,0.0}; std::vector CENTER_P = {309.11,63.29,524.27,-179.4,-0.04,0.00}; std::vector ORIGIN_COOR = {150.71,-435.9,524.27,-179.4,-0.04,-90.00}; std::vector OK_COOR = {-141.91,301.01,150,-179.4,-0.04,0.0}; std::vector NG_COOR = {-141.91,657.01,150,-179.4,-0.04,0.0}; std::vector TEST_COOR = {250.71,-480.9,424.27,-179.4,-0.04,-90.00}; std::vector STORE_COOR(6); std::vector Point(6); std::vector Point_current(6); std::vector Center_temp(6); std::vector Point_temp1(6); std::vector Point_temp2(6); std::vector 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 nova5_ptr_ = nullptr; nova5_ptr_ = std::make_shared(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(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(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(); }