pid tuning the wheel velocity

asked 2020-12-21 07:01:29 -0500

dinesh gravatar image

updated 2020-12-21 07:02:06 -0500

I am using diff drive controller to control the wheels of my robot. Here i am trying to use the control toolbox pid class for making the input velocity and velocity measured from wheel encoder to be equal. Here i have used the computeCommand() to calculate the pwm output to the motors. Here is my code:

namespace rrbot_control
{

RRBotHWInterface::RRBotHWInterface(ros::NodeHandle &nh_, urdf::Model *urdf_model)
  : ros_control_boilerplate::GenericHWInterface(nh_, urdf_model)
{
    ROS_INFO_NAMED("rrbot_hw_interface", "RRBotHWInterface Ready.");
    nhp_ = ros::NodeHandle("~");
    if (!nhp_.hasParam("my_pid/p"))
        nhp_.setParam("my_pid/p", 100.0);
    if (!nhp_.hasParam("my_pid/i"))
        nhp_.setParam("my_pid/i", 10.0);
    if (!nhp_.hasParam("my_pid/d"))
        nhp_.setParam("my_pid/d", 0.0);
    if (!nhp_.hasParam("my_pid/i_clamp_min"))
        nhp_.setParam("my_pid/i_clamp_min", -10.0);
    if (!nhp_.hasParam("my_pid/i_clamp_max"))
        nhp_.setParam("my_pid/i_clamp_max", 10.0);
    nhp_.setParam("publish_state", true);

    my_pid_.init(ros::NodeHandle(nhp_, "my_pid"), false);

    x_to_control_ = 0.0;
    ROS_INFO_STREAM("Node ready");

    min_ang = 0.00119361423;
    prev_right_wheel_pos = 0;
    prev_left_wheel_pos = 0;
    max_pwm_to_max_vel_radio = 80.7501211252;    
    time = ros::Time::now();
    Setup();
}

void RRBotHWInterface::read(ros::Duration &elapsed_time)
{
    const char* enc_data = Read();  
    std::vector<std::string> result;  
    boost::split(result, enc_data, boost::is_any_of(","));
    if(result.size()==2) {
        int curr_left_wheel_pos = atof(result[0].c_str());
        int curr_right_wheel_pos = atof(result[1].c_str());
        int delta_left_wheel_pos =  prev_left_wheel_pos - curr_left_wheel_pos;
        int delta_right_wheel_pos = prev_right_wheel_pos - curr_right_wheel_pos;
        joint_position_[0] += delta_left_wheel_pos * min_ang; 
        joint_velocity_[0] = (delta_left_wheel_pos * min_ang)/elapsed_time.toSec();
        joint_position_[1] += delta_right_wheel_pos * min_ang; 
        joint_velocity_[1] = (delta_right_wheel_pos * min_ang)/elapsed_time.toSec();
        prev_left_wheel_pos = curr_left_wheel_pos;
        prev_right_wheel_pos = curr_right_wheel_pos;
    }
}

void RRBotHWInterface::write(ros::Duration &elapsed_time)
{
  // Safety
  enforceLimits(elapsed_time);
  ros::Time tnow = ros::Time::now();
  // If any problem arises, make sure dt is > 0.0
  // as if its 0.0 the computedCommand will be 0.0
  ros::Duration dt = tnow - last_cmd_time_;

  double error1 = joint_velocity_command_[0] - joint_velocity_[0];
  double error2 = joint_velocity_command_[1] - joint_velocity_[1];

  double joint1_out_pow = my_pid1_.computeCommand(error1, dt);
  double joint2_out_pow = my_pid2_.computeCommand(error2, dt);
  std::cout<<"error1="<<error1<<"\t"<<"joint1_out_pow="<<joint1_out_pow<<"\t"<<
      "joint1_velocity_given="<<joint_velocity_command_[0]<<"\t"<<"joint1_velocity_measured="<<joint_velocity_[0]<<std::endl;

  last_cmd_time_ = tnow;
  ros::Duration(0.1).sleep();

  if(joint1_out_pow>255)
      joint1_out_pow = 255;
  else if(joint1_out_pow<-255)
      joint1_out_pow=-255;
  if(joint2_out_pow>255)
      joint2_out_pow=255;
  else if(joint2_out_pow<-255)
      joint2_out_pow=-255;
  std::string pwm_datas = std::to_string(joint1_out_pow)+","+std::to_string(joint2_out_pow);
  while(pwm_datas.length()<16) {
    pwm_datas.push_back(',');
  }
  const char *res = pwm_datas.c_str(); 
  if(pwm_datas.length()==16) {
    Write(res);    
    usleep(10000);
  }
}

Here when the input velocity is 20 the pwm is something 10... which is not rotating the wheels due to not having enough power. So the error is equal to 20-0 = 20 itself. My doubt is how do i properly use this pid controller such that the pid controller gives the pwm output which balances the input velocity and output velocity. What is the standard, right way to do it?

edit retag flag offensive close merge delete