pid controller not working in read and write

[roboticsai]

I have integrated the ros pid controllers in the read and write functions of the hardware interface of my robot like this:

RRBotHWInterface::RRBotHWInterface(ros::NodeHandle &nh, urdf::Model *urdf_model)
  : ros_control_boilerplate::GenericHWInterface(nh, urdf_model)
{
    ROS_INFO_NAMED("rrbot_hw_interface", "RRBotHWInterface Ready.");
    left_wheel_pid_controller.initPid(0.1, 0.01, 0.01, 255, -255);
    right_wheel_pid_controller.initPid(0.1, 0.01, 0.01, 255, -255);

    radi = 0.049;
    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();
    duration = ros::Duration(0.001);
    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);
  //int joint2_out_pow = max_pwm_to_max_vel_radio * joint_velocity_command_[0];  
  //int joint1_out_pow = max_pwm_to_max_vel_radio * joint_velocity_command_[1];
  double pow1_error = joint_velocity_command_[0] - joint_velocity_[0];
  double pow2_erro = joint_velocity_command_[1] - joint_velocity_[1];
  int joint1_out_pow = pid1.computeCommand(pow1_error,elapsed_time);
  int joint2_out_pow = pid1.computeCommand(pow2_error,elapsed_time);
  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);
  }
}

void RRBotHWInterface::enforceLimits(ros::Duration &period)
{
  // ----------------------------------------------------
  // ----------------------------------------------------
  // ----------------------------------------------------
  //
  // CHOOSE THE TYPE OF JOINT LIMITS INTERFACE YOU WANT TO USE
  // YOU SHOULD ONLY NEED TO USE ONE SATURATION INTERFACE,
  // DEPENDING ON YOUR CONTROL METHOD
  //
  // EXAMPLES:
  //
  // Saturation Limits ---------------------------
  //
  // Enforces position and velocity
  pos_jnt_sat_interface_.enforceLimits(period);
  //
  // Enforces velocity and acceleration limits
  // vel_jnt_sat_interface_.enforceLimits(period);
  //
  // Enforces position, velocity, and effort
  // eff_jnt_sat_interface_.enforceLimits(period);

  // Soft limits ---------------------------------
  //
  // pos_jnt_soft_limits_.enforceLimits(period);
  // vel_jnt_soft_limits_.enforceLimits(period);
  // eff_jnt_soft_limits_.enforceLimits(period);
  //
  // ----------------------------------------------------
  // ----------------------------------------------------
  // ----------------------------------------------------
}

But when i run this hardware interface with diff drive controller and publish the cmd_vel command in twist topic. The output power which is pwm in this case is not properly comming. I asked this question in ros forum they said its not the way to use the pid controllers. If no what is the correct method to use the pid controllers in the hardware interface with ros control boiler plate? Plate-form: ros noetic, ubuntu 20.04. Here i've used the read and write functions from the serial library i've used. which does the reading and writing tasks from arduino nano connected to the raspberry pi where i'm running this, where encoder data is being read and pwm to dc motors has been sent.

[AndyZe]

I think you're on the right track with implementing the PWM in write().

These two lines are suspicious:

  int joint1_out_pow = pid1.computeCommand(pow1_error,elapsed_time);
  int joint2_out_pow = pid1.computeCommand(pow2_error,elapsed_time);

You shouldn't be making the call to PID controllers yourself. That gets done automatically by ros_control. Take a look here:

https://github.com/PickNikRobotics/ros_control_boilerplate/blob/9fa14cd3d00328efa3d44d7bf4d849ce909310f0/rrbot_control/src/rrbot_hw_interface.cpp#L81

joint_position_command_[joint_id] is automatically calculated from a PID controller, you just have to use it.

[gavanderhoorn]

@AndyZe wrote:

You shouldn't be making the call to PID controllers yourself. That gets done automatically by ros_control.

not saying it's correct in this case, but there are actually closed loop hardware_interface implementations.

ros_control even comes with a few adapters for those.

[AndyZe]

Well, gavanderhoorn may well be right. I guess there is no issue with calling your own PID controllers directly. It's just a bit unusual.

Another issue:

  int joint1_out_pow = pid1.computeCommand(pow1_error,elapsed_time);
  int joint2_out_pow = pid1.computeCommand(pow2_error,elapsed_time);

Shouldn't it be pid1 and pid2? A separate controller for each joint.

[AndyZe]

I think you would also need to scale the PID controller output carefully so it is in the range of 0-255. It might be that the output of your PID controllers is tiny, like 0-1.