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

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