git commit -m "first commit for v2"
This commit is contained in:
1331
Controllers/Packages/nova5_control/src/imr_nova_control.cpp
Normal file
1331
Controllers/Packages/nova5_control/src/imr_nova_control.cpp
Normal file
File diff suppressed because it is too large
Load Diff
670
Controllers/Packages/nova5_control/src/nova5_modbus.cpp
Normal file
670
Controllers/Packages/nova5_control/src/nova5_modbus.cpp
Normal 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], ®ister_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,®ister_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, ®ister_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], ®ister_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, ®ister_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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user