- Step1——文件替换与放置
- 首先拿到新版的Unitree SDK包,我整理好了放在网盘里
- 1框四个文件夹放置在
lys_ws/src/lys_pkg/include
路径下 - 2框的SDK文件放置在
lys_ws/src/lys_pkg/src/lib
路径下
- Step2——修改CMakeLists.txt文件
- 打开
lys_ws/src/lys_pkg/
下的CMakeLists.txt
文件,将第135行左右的libUnitree_motor_SDK_Linux64.so替换为libunitreeMotorSDK_Linux64.so
(即Step1中替换的新版sdk文件)
- 打开
- Step3——修改四个a1motor_.cpp文件的头文件
-
Step3和Step4记录的比较详细,主要为了方便我自己日后回顾,仅移植需要的话直接 点这里 把四个文件替换掉就好
- 在
lys_ws\src\lys_pkg\src
下有四个与电机通信的文件,对应leg1~leg4,仅以a1motor1.cpp
为例,其他类推即可。 - 首先修改头文件引用,原版的头文件为:
-
#include <stdio.h>
#include <errno.h> //错误定义
#include <string.h>
#include <unistd.h> //Unix标准函数定义, usleep()
#include <sys/time.h>
#include "LSerial.h" //串口通信函数
#include "motor_ctrl.h" //声明发送数据、接收数据的结构体,以及函数声明
#include "check.h"
更改为:
#include <stdio.h>
#include <errno.h> //错误定义
#include <string.h>
#include <unistd.h> //Unix标准函数定义, usleep()
#include <sys/time.h>
#include "check.h"
#include "serialPort/SerialPort.h"
- Step4——函数细节修改
- 删掉16行左右的
int fd=open_set((char*)"/dev/leg1");
- 以下全是在
void A1Motor_1_Callback()
函数内的修改 - 函数第一行(35行左右)前加一行
SerialPort serial("/dev/leg1");
,注意leg的序号要与腿号对应,其他三个文件是leg2~4 - 46行左右
send_recv(fd, &motor_s, &motor_r);
改为serial.sendRecv(&motor_s, &motor_r);
- 69行左右
send_recv(fd, &motor_s, &motor_r);
改为serial.sendRecv(&motor_s, &motor_r);
- 完整修改后代码如下:
- 删掉16行左右的
#include <stdio.h>
#include <errno.h> //错误定义
#include <string.h>
#include <unistd.h> //Unix标准函数定义, usleep()
#include <sys/time.h>
#include "check.h"
#include "serialPort/SerialPort.h"
MOTOR_send motor_s, motor_s1;
MOTOR_recv motor_r;
int main(int argc, char** argv)
{
//初始化ros节点
ros::init(argc, argv, "a1motor_1_cpp");
//创建节点句柄
ros::NodeHandle nh;
A1MotorData_1_sub = nh.subscribe("A1Motor_1_data", 16, A1Motor_1_Callback);
A1MotorBack_1_pub = nh.advertise<lys_pkg::A1MotorBack_msg>("A1MotorBack_1_data",10);
ros::spin();
return 0;
}
void A1Motor_1_Callback(const lys_pkg::A1Motor_msg& msg)
{
SerialPort serial("/dev/leg1");
motor_s.id = msg.id;
motor_s.mode = 10;
motor_s.T = msg.T; //单位:Nm, T<255.9
motor_s.W = msg.W;//21.0; //单位:rad/s, W<511.9
motor_s.Pos = msg.Pos; //单位:rad, Pos<131071.9
motor_s.K_P = msg.P; //K_P<31.9
motor_s.K_W = msg.D;//10; //K_W<63.9
modify_data(&motor_s);
int sta;
if(motor_s.T == 0 && motor_s.W == 0 && motor_s.Pos == 0 && motor_s.K_P == 0 && motor_s.K_W ==0)
{
serial.sendRecv(&motor_s, &motor_r);
extract_data(&motor_r);
if(motor_s.id==0)
{
A1MotorcaliPos_[0]=motor_r.Pos;
std::cout<<"第一条腿ID为0的电机初始角为 ="<<A1MotorcaliPos_[0]<<std::endl;
a1motorback_1_msg.Pos = motor_r.Pos;
a1motorback_1_msg.id = motor_s.id;
A1MotorBack_1_pub.publish(a1motorback_1_msg);
}
else if(motor_s.id==1)
{
A1MotorcaliPos_[1]=motor_r.Pos;
std::cout<<"第一条腿ID为1的电机初始角为 ="<<A1MotorcaliPos_[1]<<std::endl;
a1motorback_1_msg.Pos = motor_r.Pos;
a1motorback_1_msg.id = motor_s.id;
A1MotorBack_1_pub.publish(a1motorback_1_msg);
}
}
else
{
serial.sendRecv(&motor_s, &motor_r);
}
}
- Step5——编译建议
- 更换好SDK后可以先直接
catkin_make
一下看看有没有报错,没有的话说明基本没有问题,理论上就可以了,但是建议做一下下方操作 - 将
lys_ws\devel\include\lys_pkg
下的三个头文件备份到桌面,然后删掉lys_ws
主工作空间下的build
和devel
两个文件夹,重新编译,这时会报错,将备份好的三个头文件重新放回上述路径,再次编译即可
- 更换好SDK后可以先直接