arduino舵机达180不到_dynamixel 舵机 ros 使用指南（一）

参考链接：

官方教程 ： http:// emanual.robotis.com/doc s/en/software/dynamixel/dynamixel_workbench/
舵机control table： http:// emanual.robotis.com/doc s/en/dxl/x/xh430-w350/#control-table-of-eeprom-
舵机通讯协议 http:// emanual.robotis.com/doc s/en/dxl/protocol2/#status-packet

首先从官网下载 dynamixel 舵机官方源码，参考官方教程：

将文件dynamixel-workbench,dynamixel-workbench-msgs,DynamixelsSDK文件夹放进工作空间catkin_ws内，其中前面两个是main packages，SDK是depend package:

Mark 1：你可以下载一个Dynamixel wizard 2 便于调试，上位机界面截图如下 （官方提供了win和linux版本）
Mark 2：如果你连接了多个舵机，只要存在舵机ID号重复的情况，上位机会检测不到，因此遇到这种情况，要先依次连接舵机然后修改舵机的ID号，使其互异。

如何编写一个简单的舵机控制程序

官方提供的workbench源代码框架比较完整，但代码的体量很大，有时为了快速验证算法，需要自己写一个简单的控制程序，当初。。。看源代码看的头晕眼花也不知道怎么用，还是偶然在一个论坛得到灵感：

首先建立自己的功能包，添加的依赖：

find_package(catkin REQUIRED COMPONENTS
  dynamixel_workbench_toolbox
  roscpp
  std_msgs
)

在cpp文件中是引入 dynamixel_workbench_toolbox这个API，这个API定义了官方提到的一些封装好的函数

#include<dynamixel_workbench_toolbox/dynamixel_workbench.h>
#include<ros/ros.h>

基本使用的方法如下（更多函数的用法参照官网，或者源文件）

DynamixelWorkbench dxl_wb;  //声明对象
dxl_wb.begin("/dev/ttyUSB0",BAUDRATE);  //打开串口
dxl_wb.setPositionControlMode(ID);  //设置位置控制模式
dxl_wb.torqueOn(ID);    //上电，只有断电的情况下才能设置控制模式
dxl_wb.itemRead(ID,"Present_Position",&present_position);   //读取当前的位置
dxl_wb.itemWrite(ID,"Goal_Position",goal_position);         //写入目标的位置
// 这里的position current velocity 都是以int32_t的形式存储

下面案例即实现一个简单的PID控制

#include<dynamixel_workbench_toolbox/dynamixel_workbench.h>
#include<ros/ros.h>

#define BAUDRATE 57600
#define ID 0


int main(int argc,char **argv)
{
  ros::init(argc,argv,"my_dxl_node");
  ros::NodeHandle n;
  

  DynamixelWorkbench dxl_wb; 
  dxl_wb.begin("/dev/ttyUSB0",BAUDRATE);

  ROS_INFO("Welcome my dynamixel workbench!");
  //feedback info
  float radian = 0.0;
  int32_t position_data = 0.0;
  float velocity = 0.0;
  int32_t velocity_data = 0;
  float current = 0.0;
  int32_t current_data = 0;
  int limit_current = 0;

  dxl_wb.itemRead(ID,"Current_Limit",&limit_current);
  ROS_INFO("Current Limit:%d",limit_current);

  dxl_wb.setCurrentControlMode(ID); // you can change control mode only if the state is torque-off.
  dxl_wb.torqueOn(ID);

  //pid current control 
  int goal_position = 2000;
  int goal_current = 0;
  int position_err = 0;
  int last_position_err = 0;
  int err_integral = 0;
  float p_gain = 0.0325;
  float i_gain = 0.0;
  float d_gain = 0.05;
  int present_position = 0;


  ros::Rate loop_rate(10);

  while(ros::ok())
  {
   //get dxl state include current velocity & radian
   dxl_wb.itemRead(ID,"Present_Position",&present_position);
   dxl_wb.getRadian(ID,&radian);
   dxl_wb.itemRead(ID,"Present_Velocity",&velocity_data);
   velocity= dxl_wb.convertValue2Velocity(ID,velocity_data);  
   dxl_wb.itemRead(ID,"Present_Current",&current_data);
   current = dxl_wb.convertValue2Current(ID,current_data);
   ROS_INFO("radian:%2.2f degree velocity:%0.3f rpm current:%0.2f mA present_position:%d",radian/3.14*180+180,velocity,current,present_position); 
   /****************************/
   //pid control; first step realise p control
   //Expect:goal_position feedback:present_position Input:goal_current output:present_position
   /****************************/
   last_position_err = position_err;
   position_err = goal_position - present_position;
   err_integral += position_err;
   goal_current = (int) (p_gain * position_err+ i_gain * err_integral +d_gain * (position_err-last_position_err));
   if(goal_current>limit_current)
   {
      goal_current = limit_current;
   }
   if(goal_current<-limit_current)
   {
      goal_current = -limit_current;
   }
   ROS_INFO("goal_current:%d",goal_current);

  // dxl_wb.itemWrite(ID,"Goal_Current",goal_current);
   dxl_wb.itemWrite(ID,"Goal_Position",goal_position);
   //output current 
   loop_rate.sleep();
  }
  return 0;
}