0
$\begingroup$

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?

$\endgroup$
1
$\begingroup$

If you're using a full PID controller then the integral error term should build until the output gets large enough to turn the wheel. I can't see your PID code so I'm not sure how your error terms are calculated, and I also can't see how that integral clamp is getting applied.

I would recommend against doing any kind of clamping on integral error terms unless you find some really good reason to use it - the integral term is what eliminates steady state error. It should be allowed to grow as big as necessary to do that.

Finally, as a side note, I thought it was odd that you are limiting the output to +/- 255. An 8-bit unsigned value is 0 to 255, but a signed 8-bit value is - 127 to 128. You should double check what the acceptable output range is. You might be correct but I thought it was odd.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.