HiveCoreRD/hivecore_robot_drivers
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update img dev config

Browse Source
This commit is contained in:
NuoDaJia02
2025-12-28 17:16:26 +08:00
parent afa2b443f5
commit 1a6fe5df70
6 changed files with 333 additions and 177 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
gripper_dev/gripper_dev/gripper_dev_node.py
View File

@@ -16,7 +16,7 @@ class GripperDevNode(Node):
self.declare_parameter('port','/dev/ttyUSB0')
param = self.get_parameter('port')
self.com_dev = param.value
self.declare_parameter('devid',9)
self.declare_parameter('devid',6)
param = self.get_parameter('devid')
self.devid = param.value
@@ -60,26 +60,76 @@ class GripperDevNode(Node):
self.clawControl.runWithParam(self.devid,self.target_loc,self.target_speed,self.target_torque)
##feedback
loop_flag=True
loop_times=0
origin_loc = self.clawControl.getClampCurrentLocation(self.devid)
origin_torque = self.clawControl.getClampCurrentTorque(self.devid)
loc_direction = 0
tor_direction = 0
if (origin_loc[0] < self.target_loc) :loc_direction = 1
if (origin_torque[0] < self.target_torque) :tor_direction = 1
print("origin loc: ", origin_loc[0], self.target_loc, loc_direction)
print("origin torque: ", origin_torque[0], self.target_torque, tor_direction)
while loop_flag:
loc = self.clawControl.getClampCurrentLocation(self.devid)
speed = self.clawControl.getClampCurrentSpeed(self.devid)
torque = self.clawControl.getClampCurrentTorque(self.devid)
self.cur_loc=loc[0]
self.cur_speed=speed[0]
if self.target_mode==0:
loop_flag=abs(self.cur_loc-self.target_loc)>2
elif self.target_mode==1:
loop_flag=abs(self.cur_torque-self.target_torque)>2
else:
loop_flag=abs(self.cur_loc-self.target_loc)>2 and abs(self.cur_loc-self.target_loc)>2
#print('feedback:',self.cur_loc,speed,torque)
self.cur_torque = torque[0]
if self.target_mode==0: # loc mode
# loop_flag=abs(self.cur_loc-self.target_loc)>2
if (loc_direction) :
loop_flag = False if (self.cur_loc >= self.target_loc) or (self.target_loc - self.cur_loc) < 2 else True
else :
loop_flag = False if (self.cur_loc <= self.target_loc) or (self.cur_loc - self.target_loc) < 2 else True
elif self.target_mode==1: # torque mode
# loop_flag=abs(self.cur_torque-self.target_torque)>2
if (tor_direction) :
loop_flag = False if (self.cur_torque >= self.target_torque) or (self.target_torque - self.cur_torque) < 2 else True
else :
loop_flag = False if (self.cur_torque <= self.target_torque) or (self.cur_torque - self.target_torque) < 2 else True
else: #loc or torque
# loop_flag=abs(self.cur_loc-self.target_loc)>2 and abs(self.cur_torque-self.target_torque)>2
if (loc_direction) :
loc_loop_flag = False if (self.cur_loc >= self.target_loc) or (self.target_loc - self.cur_loc) < 2 else True
else :
loc_loop_flag = False if (self.cur_loc <= self.target_loc) or (self.cur_loc - self.target_loc) < 2 else True
if (tor_direction) :
tor_loop_flag = False if (self.cur_torque >= self.target_torque) or (self.target_torque - self.cur_torque) < 2 else True
else :
tor_loop_flag = False if (self.cur_torque <= self.target_torque) or (self.cur_torque - self.target_torque) < 2 else True
print("loc_loop_flag: ", loc_loop_flag)
print("tor_loop_flag: ", tor_loop_flag)
loop_flag = loc_loop_flag and tor_loop_flag
print("mode: ", self.target_mode)
print("torque: ", self.cur_torque, self.target_torque)
print("loc: ", self.cur_loc, self.target_loc)
print("loop_flag: ", loop_flag)
print("loc_direction: ", loc_direction)
print("tor_direction: ", tor_direction)
# print('feedback:', self.cur_loc, speed, torque)
feedback_msg = GripperCmd.Feedback()
feedback_msg.loc=loc[0]
feedback_msg.speed=speed[0]
feedback_msg.torque=torque[0]
print('update feedback:',feedback_msg)
print('update feedback:', feedback_msg)
self.cur_goal.publish_feedback(feedback_msg)
time.sleep(1)
loop_times+=1
if loop_times>15:
result=GripperCmd.Result()
result.state="loop timeout!!!"
result.result=0
goal.abort()
loop_times=0
print('loop timeout',state)
return result
else:
result=GripperCmd.Result()
result.state="uart open fail!"

4
img_dev/launch/img_dev.launch.py
View File

@@ -9,8 +9,8 @@ def generate_launch_description():
# 启动orbbec_camera
#ExecuteProcess(cmd=['ros2','launch', 'orbbec_camera', 'multi_camera.launch.py'],output='screen'),
ExecuteProcess(cmd=['ros2','launch', 'orbbec_camera', 'gemini_330_series.launch.py'],output='screen'),
# ExecuteProcess(cmd=['sleep', '2'],output='screen'),
#ExecuteProcess(cmd=['ros2','launch', 'realsense2_camera','rs_launch.py'],output='screen'),
ExecuteProcess(cmd=['sleep', '2'],output='screen'),
ExecuteProcess(cmd=['ros2','launch', 'realsense2_camera','rs_launch.py'],output='screen'),
ExecuteProcess(cmd=['sleep', '2'],output='screen'),

8
img_dev/src/main.cpp
View File

@@ -15,11 +15,11 @@ using namespace std;
using namespace img_dev;
using SyncPolicy=message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image,sensor_msgs::msg::Image>;
static shared_ptr<ImageSubscriber> cur_node=nullptr;
ImgCfg cfg0=ImgCfg("orbbec", "myType","left","/camera/color/image_raw","/camera/depth/image_raw");
ImgCfg cfg1=ImgCfg("orbbec", "myType","top","/camera_02/color/image_raw","/camera_02/depth/image_raw");
ImgCfg cfg0=ImgCfg("orbbec", "myType","top","/camera/color/image_raw","/camera/depth/image_raw");
ImgCfg cfg1=ImgCfg("orbbec", "myType","null","/camera_02/color/image_raw","/camera_02/depth/image_raw");
//
ImgCfg cfg2=ImgCfg("realsense", "myType","right","/camera/camera/rgbd","/camera/camera/rgbd");
ImgCfg cfg3=ImgCfg("realsense", "myType","hand_r","/camera2/camera2/rgbd","/camera2/camera2/rgbd");
ImgCfg cfg3=ImgCfg("realsense", "myType","left","/camera2/camera2/rgbd","/camera2/camera2/rgbd");
void sync_cb0(const sensor_msgs::msg::Image& c_img, const sensor_msgs::msg::Image& d_img) {
cur_node->pub_msg(c_img,d_img,cfg0);
}
@@ -35,7 +35,7 @@ int main(int argc, char* argv[]) {
return -1;
}
#if 0
#if 1
//////////////////奥比中光1 START///////////////////////
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera01_info=cur_node->create_subscription<sensor_msgs::msg::CameraInfo>("/camera/color/camera_info", 10,
[&](const sensor_msgs::msg::CameraInfo& info){

11
motor_dev/include/motor_dev/rs485_driver.hpp
View File

@@ -5,7 +5,7 @@
#include <vector>
#include <termios.h>
#include <mutex>
#include<map>
namespace motor_dev
{
class RS485Driver
@@ -16,7 +16,7 @@ namespace motor_dev
// 打开串口
bool openPort(const std::string &port_name, int baud_rate = 115200);
std::map<uint8_t, float> speedMap;
// 关闭串口
void closePort();
@@ -25,14 +25,13 @@ namespace motor_dev
// 接收数据
bool receiveData(std::vector<uint8_t> &data, size_t max_length, int timeout_ms = 100);
std::vector<uint8_t> sendCan(uint8_t cmd,const std::vector<uint8_t> data);
//本末
int bm_set_enable(uint8_t enable);
int bm_set_enable(uint8_t motor_id,uint8_t enable);
int bm_set_mode(uint8_t motor_id, uint8_t mode);
int bm_set_pos(float angle1, float angle2);
int bm_set_speed(float speed1, float speed2,float speed3,float speed4);
int bm_set_speed(float speed1, float speed2);
float bm_get_pos(uint8_t motor_id);
int bm_get_speed();
float bm_get_speed(uint8_t motor_id);
int bm_query_id();
int bm_set_param(uint8_t motor_id, uint8_t param,uint32_t val);

156
motor_dev/src/main.cpp
View File

@@ -3,95 +3,171 @@
#include"interfaces/msg/motor_cmd.hpp"
#include"interfaces/msg/motor_pos.hpp"
#include"interfaces/srv/motor_param.hpp"
#include"interfaces/srv/motor_info.hpp"
using namespace motor_dev;
RS485Driver rs485_driver_;
static auto g_program_start_time = std::chrono::steady_clock::now();
// 带时间戳的 printf 风格日志函数
void log_printf(const char* format, ...) {
auto now = std::chrono::steady_clock::now();
auto elapsed = std::chrono::duration_cast<std::chrono::duration<double>>(now - g_program_start_time);
double seconds = elapsed.count();
char buffer[1024];
va_list args;
va_start(args, format);
int n = vsnprintf(buffer, sizeof(buffer), format, args);
va_end(args);
if (n < 0) {
std::fprintf(stderr, "[%.3f] (log error)\n", seconds);
return;
}
std::fprintf(stdout, "[%.3f] %s\n", seconds, buffer);
std::fflush(stdout); // 确保立即输出（对调试很重要）
}
int main(int argc,char* argv[]){
rclcpp::init(argc,argv);
auto node=rclcpp::Node::make_shared("motor_dev_node");
////auto pos_pub = node->create_publisher<motor_dev::msg::MotorPos>("/motor_pos", 10);
std::cout << "open RS485 port.." << std::endl;
if (!rs485_driver_.openPort("/dev/ttyACM0", 921600)){
if (!rs485_driver_.openPort("/dev/ttyS1", 115200)){
std::cout << "Failed to open RS485 port." << std::endl;
}
usleep(100000);
rs485_driver_.speedMap.clear();
rs485_driver_.bm_set_enable(1);
rs485_driver_.bm_set_param(1,28,3);
rs485_driver_.bm_set_param(2,28,3);
rs485_driver_.bm_set_param(3,28,3);
rs485_driver_.bm_set_param(4,28,3);
rs485_driver_.bm_set_enable(2);
rs485_driver_.bm_set_param(1,84,100);
rs485_driver_.bm_set_param(2,84,100);
rs485_driver_.bm_set_param(3,84,100);
rs485_driver_.bm_set_param(4,84,100);
usleep(500000);
rs485_driver_.bm_set_enable(1,1);
rs485_driver_.bm_set_enable(2,1);
rs485_driver_.bm_set_mode(1,3);//3应该是速度模式，我记得
rs485_driver_.bm_set_enable(1,2);
rs485_driver_.bm_set_mode(2,3);
rs485_driver_.bm_set_enable(2,2);
//下面这几个参数我记得是跟位置模式有关，也许可以不用
// rs485_driver_.bm_set_param(1,77,51);
// rs485_driver_.bm_set_param(1,78,8);
// rs485_driver_.bm_set_param(1,79,8);
// //rs485_driver_.bm_set_param(1,28,4);
// rs485_driver_.bm_set_param(2,77,51);
// rs485_driver_.bm_set_param(2,78,8);
// rs485_driver_.bm_set_param(2,79,8);
//rs485_driver_.mt_set_pos(1,-5);
//init
/////rs485_driver_.bm_set_mode(1,3);//位置模式
auto rs485_sub_=node->create_subscription<interfaces::msg::MotorCmd>("/motor_cmd",10,[](const interfaces::msg::MotorCmd::SharedPtr msg){
///
/// printf("##############-----------------------");
if (msg->target == "rs485") {
if(msg->type=="bm"){
size_t n = msg->motor_id.size();
if(n<2){
printf("bm msg need four angle\n");
printf("bm msg need two angle\n");
return;
}
//float angle1 =msg->motor_angle[0];
//float angle2 =msg->motor_angle[1];
float speed1=msg->motor_speed[0];
float speed2=msg->motor_speed[1];
float speed3=msg->motor_speed[3];
float speed4=msg->motor_speed[4];
printf("###:%s,speed:%.1f,%.1f,%.1f,%.1f\n",msg->type.c_str(),speed1,speed2,speed3,speed4);
rs485_driver_.bm_set_speed(speed1,speed2,speed3,speed4);
//log_printf("recv topic:%s,angle:%.1f,%.1f",msg->type.c_str(),angle1,angle2);
log_printf("recv topic:%s,speed:%.1f,%.1f",msg->type.c_str(),speed1,speed2);
///rs485_driver_.bm_set_pos(angle1,angle2);
rs485_driver_.bm_set_speed(speed1,speed2);
}else if(msg->type=="mt"){
uint8_t id=msg->motor_id[0];
float angle =msg->motor_angle[0];
rs485_driver_.mt_set_pos(id,angle);
}
}else if(msg->target=="ethercat"){
//TODO
}
});
#if 1
auto motor_pub=node->create_publisher<interfaces::msg::MotorPos>("/motor_pos",10);
//读取电机位置
rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(5000), [=]() {
rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(2000), [=]() {
static int pub_cnt=0;
// log_printf("query motor pos");
interfaces::msg::MotorPos motor_pos;
rs485_driver_.bm_get_speed();
//float angle1=rs485_driver_.bm_get_pos(1);
//float angle2=rs485_driver_.bm_get_pos(2);
float speed1=rs485_driver_.bm_get_speed(1);
float speed2=rs485_driver_.bm_get_speed(2);
///printf("bm cur angle:%.1f\n",angle);
///rs485_driver_.bm_query_id();
//rs485_driver_.mt_query_id();//3e cd 08 79 00 01 00 00 00 00 00 f8 95
motor_pos.motor_id.clear();
motor_pos.motor_angle.clear();
//motor_pos.motor_angle.clear();
motor_pos.motor_speed.clear();
////printf("pub msg[%03d],%.1f,%.1f\n",pub_cnt,angle1,angle2);
//if(angle1>-1&&angle2>-1)
{
motor_pos.source = "rs485";
motor_pos.type = "bm";
motor_pos.position="none";
for(const auto& kv:rs485_driver_.speedMap){
printf("%d:%.1f,",kv.first,kv.second);
motor_pos.motor_id.push_back(kv.first);
motor_pos.motor_speed.push_back(kv.second);
}
printf("\n");
motor_pos.motor_id.push_back(1);
motor_pos.motor_id.push_back(2);
//motor_pos.motor_angle.push_back(angle1);
//motor_pos.motor_angle.push_back(angle2);
motor_pos.motor_speed.push_back(speed1);
motor_pos.motor_speed.push_back(speed2);
motor_pub->publish(motor_pos);
//std::cout<<"pub msg["<<pub_cnt<<"]:"<<angle1<<","<<angle2<<std::endl;
// log_printf("pub pos:%.1f,%.1f",angle1,angle2);
}
pub_cnt+=1;
#if 0
motor_pos.motor_id.clear();
motor_pos.motor_angle.clear();
float angle=rs485_driver_.mt_get_pos(1);
if(angle!=9999){
motor_pos.type = "mt";
motor_pos.motor_id.push_back(1);
motor_pos.motor_angle.push_back(angle);
motor_pub->publish(motor_pos);
}
#endif
});
#endif
auto motor_param_srv=node->create_service<interfaces::srv::MotorParam>("motor_param",[=](const std::shared_ptr<interfaces::srv::MotorParam::Request> req,std::shared_ptr<interfaces::srv::MotorParam::Response> res){
res->ret=0;
printf("recv sv:%d,%d\n",req->motor_id,req->accel);
if(req->accel<500){
rs485_driver_.bm_set_param(1,84,300);
res->ret=1;
printf("svc:%d\n",res->ret);
uint8_t mid=req->motor_id;
res->ret=-1;
log_printf("recv sv:%d,%d,%d,%d",req->motor_id,req->max_speed,req->add_acc,req->dec_acc);
//if(mid>-1&&mid<500)
{
// if(req->max_speed<500&&req->add_acc<500&&req->dec_acc<500){
// rs485_driver_.bm_set_param(mid,77,req->max_speed);
// rs485_driver_.bm_set_param(mid,78,req->add_acc);
// rs485_driver_.bm_set_param(mid,79,req->dec_acc);
// res->ret=0;
// printf("svc:%d\n",res->ret);
// }
}
});
#if 0
auto motor_info_srv=node->create_service<interfaces::srv::MotorInfo>("motor_info",[=](const std::shared_ptr<interfaces::srv::MotorInfo::Request> req,std::shared_ptr<interfaces::srv::MotorInfo::Response> res){
printf("recv motor_info:%s,%d\n",req->type.c_str(),req->motor_ids[0]);
res->ret=false;
if(req->type=="bm"){
res->motor_angles.clear();
for(auto d:req->motor_ids){
float angle=rs485_driver_.bm_get_pos(d);
res->motor_angles.push_back(angle);
printf("motor_info %d, angle:%.1f\n",d,angle);
}
res->ret=true;
}
});
#endif
rclcpp::spin(node);
printf("now close rs485 port\n");

263
motor_dev/src/rs485_driver.cpp
View File

@@ -10,52 +10,6 @@
namespace motor_dev
{
std::vector<uint8_t> RS485Driver::sendCan(uint8_t cmd,const std::vector<uint8_t> data) {
std::vector<uint8_t> packet;
packet.push_back(0x55); // 同步字节1
packet.push_back(0xAA); // 同步字节2
packet.push_back(0x1E); // 30字节
packet.push_back(0x01);
packet.push_back(0x01);
packet.push_back(0x00);
packet.push_back(0x00);
packet.push_back(0x00);
packet.push_back(0x0a);
packet.push_back(0x00);
packet.push_back(0x00);
packet.push_back(0x00);
packet.push_back(0x00);
packet.push_back(cmd);
packet.push_back(0x00);
packet.push_back(0x00);
packet.push_back(0x00);
packet.push_back(0x00);
packet.push_back(0x08);
packet.push_back(0x00);
packet.push_back(0x00);
for (uint8_t i = 0; i < 8; i++) {
packet.push_back(data[i]);
}
packet.push_back(0x00);
bool ret = sendData(packet);
std::vector<uint8_t> rx_data;
if(ret){
rx_data.clear();
ret=receiveData(rx_data, 100, 1);
if(ret){
printf("<---:");
for(auto d:rx_data)
printf("%02x ",d);
printf("\n");
return rx_data;
}
}
return rx_data;
}
const unsigned char CRC8Table[]={
0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,
157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,
@@ -220,10 +174,12 @@ namespace motor_dev
{
static int idx=0;
idx+=1;
#ifdef hehe
printf("[%03d]-->",idx);
for(auto d:data)
printf("%02x ",d);
printf("\n");
#endif
ssize_t bytes_written = write(com_fd_, data.data(), data.size());
if (bytes_written != static_cast<ssize_t>(data.size()))
{
@@ -294,7 +250,7 @@ namespace motor_dev
bool ret = sendData(command);
if(ret){
std::vector<uint8_t> data;
bool ret=receiveData(data, 20, 1);
bool ret=receiveData(data, 20, 20);
if(ret){
printf("recv:%02x,%02x,%02x,%02x\n",data[0],data[1],data[2],data[3]);
}else{
@@ -313,19 +269,19 @@ namespace motor_dev
command.push_back(0x36);
command.push_back(motor_id);
command.push_back(0x1c);
command.push_back(mode);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(2);
command.push_back(0);
command.push_back(0);
//uint8_t crc8 = CRC8_Table(command.data(), 10);
//command.push_back(crc8);
uint8_t crc8 = CRC8_Table(command.data(), 10);
command.push_back(crc8);
bool ret = sendData(command);
if(ret){
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 1);
ret=receiveData(data, 20, 20);
if(ret){
printf("<---:");
for(auto d:data)
@@ -337,6 +293,65 @@ namespace motor_dev
}
int RS485Driver::bm_set_param(uint8_t motor_id, uint8_t param,uint32_t val){
std::vector<uint8_t> packet;
packet.push_back(0);
packet.push_back(0x36);
packet.push_back(motor_id);
packet.push_back(param);
packet.push_back((uint8_t)(val));
packet.push_back((uint8_t)(val>>8));
packet.push_back((uint8_t)(val>>16));
packet.push_back((uint8_t)(val>>24));
packet.push_back(0);
packet.push_back(0);
uint8_t crc8 = CRC8_Table(packet.data(), 10);
packet.push_back(crc8);
bool ret = sendData(packet);
if(ret){
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 20);
if(ret){
printf("<---:");
for(auto d:data)
printf("%02x ",d);
printf("\n");
}
}
return 0;
}
int RS485Driver::bm_set_enable(uint8_t motor_id,uint8_t enable){
std::vector<uint8_t> command;
command.push_back(0);
command.push_back(0x38);
command.push_back(motor_id);
command.push_back(enable);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
uint8_t crc8 = CRC8_Table(command.data(), 10);
command.push_back(crc8);
bool ret = sendData(command);
if(ret){
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 20);
if(ret){
printf("<---:");
for(auto d:data)
printf("%02x ",d);
printf("\n");
}
}
return 0;
}
int RS485Driver::bm_set_pos(float angle1,float angle2){
std::vector<uint8_t> command;
//if(angle>360.0)
@@ -362,10 +377,11 @@ namespace motor_dev
unsigned char crc8 = CRC8_Table(command.data(), 10);
command.push_back(crc8);
bool ret = sendData(command);
#if 0
if(ret){
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 1);
ret=receiveData(data, 20, 20);
if(ret){
printf("<---:");
for(auto d:data)
@@ -373,62 +389,47 @@ namespace motor_dev
printf("\n");
}
}
#endif
return 0;
}
int RS485Driver::bm_set_speed(float speed1,float speed2,float speed3,float speed4){
int RS485Driver::bm_set_speed(float speed1,float speed2){
std::vector<uint8_t> command;
//if(angle>360.0)
// angle=360.0;
int32_t pos=speed1*10;
uint8_t high1 = (pos >> 8) & 0xFF; // 高8位
uint8_t low1 = pos & 0xFF; // 低8位
pos=speed2*10;
uint8_t high2 = (pos >> 8) & 0xFF; // 高8位
uint8_t low2 = pos & 0xFF; // 低8位
pos=speed3*10;
uint8_t high3 = (pos >> 8) & 0xFF; // 高8位
uint8_t low3 = pos & 0xFF; // 低8位
pos=speed4*10;
uint8_t high4 = (pos >> 8) & 0xFF; // 高8位
uint8_t low4 = pos & 0xFF; // 低8位
command.push_back(high1);
command.push_back(low1);
command.push_back(high2);
command.push_back(low2);
command.push_back(high3);
command.push_back(low3);
command.push_back(high4);
command.push_back(low4);
printf("set_speed.%.1f\n",speed1);
sendCan(0x32,command);
return 0;
}
int RS485Driver::bm_set_enable(uint8_t enable){
std::vector<uint8_t> command;
command.push_back(enable);
command.push_back(enable);
command.push_back(enable);
command.push_back(enable);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
printf("set_enable\n");
sendCan(0x38,command);
return 0;
}
int RS485Driver::bm_set_param(uint8_t motor_id, uint8_t param,uint32_t val){
std::vector<uint8_t> command;
command.push_back(motor_id);
command.push_back(param);
command.push_back((uint8_t)(val));
command.push_back((uint8_t)(val>>8));
command.push_back((uint8_t)(val>>16));
command.push_back((uint8_t)(val>>24));
command.push_back(0);
command.push_back(0);
printf("set_param:%d,%d,%d\n",motor_id,param,val);
sendCan(0x36,command);
//if(motor_id<5){
command.push_back(0x00);
command.push_back(0x32);
command.push_back(high1);
command.push_back(low1);
command.push_back(high2);
command.push_back(low2);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
//}
unsigned char crc8 = CRC8_Table(command.data(), 10);
command.push_back(crc8);
bool ret = sendData(command);
if(ret){
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 5);
if(ret){
#ifdef hehe
printf("<---:");
for(auto d:data)
printf("%02x ",d);
printf("\n");
#endif
}
}
return 0;
}
float RS485Driver::bm_get_pos(uint8_t motor_id){
@@ -449,12 +450,14 @@ namespace motor_dev
if(ret){
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 1);
ret=receiveData(data, 11, 1);
if(ret){
#ifdef hehe
printf("<---:");
for(auto d:data)
printf("%02x ",d);
printf("\n");
#endif
uint8_t nid=0x70+motor_id;
if(data[0]==0&&data[1]==nid){
//uint16_t pos = (data[2]<<8)+(data[3]&0xff);
@@ -475,26 +478,54 @@ namespace motor_dev
}
return -1;
}
int RS485Driver::bm_get_speed(){
float RS485Driver::bm_get_speed(uint8_t motor_id){
std::vector<uint8_t> command;
command.push_back(0);
command.push_back(0x35);
command.push_back(motor_id);
//command.push_back(0x0d);
//command.push_back(4);
//command.push_back(0x0b);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(1);
command.push_back(5);
command.push_back(11);
command.push_back(13);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
std::vector<uint8_t> rx=sendCan(0x35,command);
speedMap.clear();
for(int x=0;x<rx.size();x++)
if(rx[x]==0xaa&&rx[x+1]==0x11&&rx[x+2]==0x08){
uint8_t motor_id=rx[x+3]-0x70;
int16_t speed=(rx[x+7]<<8)+(rx[x+8]&0xff);
printf("id:%d,speed:%d\n",motor_id,speed);
speedMap[motor_id]=speed/10.0f;
unsigned char crc8 = CRC8_Table(command.data(), 10);
command.push_back(crc8);
bool ret = sendData(command);
if(ret){
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 50);
if(ret){
printf("<---:");
for(auto d:data)
printf("%02x ",d);
printf("\n");
uint8_t nid=0x70+motor_id;
if(data[0]==0&&data[1]==nid){
//uint16_t pos = (data[2]<<8)+(data[3]&0xff);
//float angle=360.0*pos/32768.0;
int16_t pos=(data[6]<<8)+(data[7]&0xff);
float angle=3.60f*pos;
pos=(data[8]<<8)+(data[9]&0xff);
float speed=pos/10.0f;
//printf("id:%d,pos:%d\n",motor_id,pos);
return speed;
///printf("pos:%d,angle:%.1f\n",pos,angle);
}
else{
}
//float angle = pos * 360.0 /255.0;
///float angle=0;
}
return 0;
}
return -1;
}
//mt
void RS485Driver::add_crc16(std::vector<uint8_t>& data) {
@@ -559,7 +590,7 @@ namespace motor_dev
#if 1
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 1);
ret=receiveData(data, 20, 20);
if(ret){
if(data[0]==0x3e&&data[1]==motor_id&&data[3]==0x92){
uint32_t pos = (data[10]<<24)+(data[9]<<16)+(data[8]<<8)+(data[7]&0xff);
@@ -600,7 +631,7 @@ namespace motor_dev
bool ret=sendData(packet);
if(ret){
std::vector<uint8_t> data;
ret=receiveData(data, 20, 1);
ret=receiveData(data, 20, 20);
if(ret){
uint8_t id= data[10];
return id;