control toolbox pid not working
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.
Seeing as this class (and its siblings) is being used by many, many people worldwide every day, my first assumption would not be that the "control toolbox pid [is] not working", but there is something wrong with the way you're trying to use it.
so what is the correct way to use it? and which function updatePid() or calculateCommand() is the one to calculate output power/velocity?