control toolbox pid not working

edit

I am using pid class of control toolbox. But the output power computer from this is not comming correctly:

#include <medicbot_control/medicbot_hw_interface.h>
#include <exception>
#include <algorithm>
#include <iostream>

#include <bits/stdc++.h> 
#include <boost/algorithm/string.hpp>

namespace rrbot_control
{

std::string to_format(const int number,int decPlace) {
    std::stringstream ss;
    ss << std::setw(decPlace) << std::setfill('0') << number;
    return ss.str();
}

RRBotHWInterface::RRBotHWInterface(ros::NodeHandle &nh, urdf::Model *urdf_model)
  : ros_control_boilerplate::GenericHWInterface(nh, urdf_model)
{
    ROS_INFO_NAMED("rrbot_hw_interface", "RRBotHWInterface Ready.");
    pid1.initPid(6.0, 1.0, 2.0, 0.3, -0.3);
    pid2.initPid(6.0, 1.0, 2.0, 0.3, -0.3);
    time = ros::Time::now();
    Setup();
}

void RRBotHWInterface::read(ros::Duration &elapsed_time)
{
    const char* enc_ticks = Read();
    std::vector<std::string> result; 
    boost::split(result, enc_ticks, boost::is_any_of(",")); 
    for (int joint_id = 0; joint_id < result.size(); joint_id++) {
         joint_velocity_[joint_id] = atof(result[joint_id].c_str());
         joint_position_[joint_id] += joint_velocity_[joint_id] * elapsed_time.toSec();
   }
}

void RRBotHWInterface::write(ros::Duration &elapsed_time)
{
  // Safety
  enforceLimits(elapsed_time);

  // ----------------------------------------------------
  // ----------------------------------------------------
  // ----------------------------------------------------
  //
  // FILL IN YOUR WRITE COMMAND TO USB/ETHERNET/ETHERCAT/SERIAL ETC HERE
  //
  // FOR A EASY SIMULATION EXAMPLE, OR FOR CODE TO CALCULATE
  // VELOCITY FROM POSITION WITH SMOOTHING, SEE
  // sim_hw_interface.cpp IN THIS PACKAGE
  //
  // DUMMY PASS-THROUGH CODE
  int joint1_out_pow = pid1.updatePid(joint_velocity_command_[0]-joint_velocity_[0], elapsed_time);
  int joint2_out_pow = pid2.updatePid(joint_velocity_command_[1]-joint_velocity_[1], elapsed_time);
  std::cout<<"joint="<<joint1_out_pow<<"\t"<<joint2_out_pow<<std::endl;
  std::string data = std::to_string(joint1_out_pow)+","+std::to_string(joint2_out_pow);
  while(data.length()<8) {
    data.push_back(',');
  }
  const char *res = data.c_str();
  if(data.length()==8) {
    Write(res);
    std::cout<<"Sent data:"<<res<<std::endl;
    usleep(10000);
  }
  // END DUMMY CODE
  //
  // ----------------------------------------------------
  // ----------------------------------------------------
  // ----------------------------------------------------
}

i'm using ubuntu 20, ros noetic. Here i'm following this api and example.