- You have mentioned about two ROS nodes which are implemented in c++. One node is to control the motor and another one is to the PID controller. What is the role of the Simulink ROS model in this setup ? Is this theird node ? Can you elaborate on this ?
- It is also worth if you can simply the problem with a setup that we can reproduce on our end to help you further. I mean provide us the simple model where you reproduce the issue.
- Can you also try without external mode, i mean simply using the Normal mode Simulation. Please note that normal model simulation also, you can Read the ROS topics and publish the data.
Issues using Closed-Loop PID autotuner
4 views (last 30 days)
Show older comments
I am trying to autotune a PID controllers for Dynamixel MX-106 motors. I need to control the position by adjusting torque. I'm using ROS to run the rest of this project and the PID controller I need to tune is implemented in C++ using ROS Noetic. There are two relevant ROS nodes.
The first controls the actual motors and listens to a topic /torques that is published by the PID controller node and adjusts them accordingly. It also publishes the positions of the motors. I am controlling 6 different motors, but they each have their own controller and I've changed the code here to only work with one for testing purpouses. I've also added a subscriber to a ROS topic /disturb, which allows MatLab to write inputs the same way as my own PID controller. I'm removed a lot of code for readbility and this will not run as is, but would be happy to upload the rest if needed.
void disturb(const publisher::Position msg)
{
float oldTorque = torques[msg.motor-1];
float torque = msg.pos;
if(torque > maxtorque)
{
//printf("Torque %g greater than max torque %d. Setting torque to %d\n", torque, maxtorque, maxtorque);
torque = maxtorque;
}
else if(torque < -maxtorque)
{
//printf("Torque %g less than min torque %d. Setting torque to %d\n", torque, -maxtorque, -maxtorque);
torque = -maxtorque;
}
//DMX torque: 0-1024 clockwise, 1024-2048 counterclockwise
if(torque < 0)
{
torque = -torque + 1024;
}
torques[msg.motor-1] = torque;
printf("DISTURBANCE motor %d torque %g -> %g\n", msg.motor, oldTorque, torques[msg.motor-1]);
}
void newTorque(const publisher::Position msg)
{
float oldTorque = torques[msg.motor-1];
torques[msg.motor-1] = msg.pos;
//printf("Motor %d torque %g -> %g\n", msg.motor, oldTorque, torques[msg.motor-1]);
}
int main(int argc, char **argv)
{
dmxWrite(1, DXL6_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
dmxWrite(1, DXL6_ID, ADDR_MX_TORQUE_CONTROL_MODE, TORQUE_ENABLE);
//-------------------ROS SETUP---------------------------//
ros::init(argc, argv, "torque_writer");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("torques", 6, newTorque);
ros::Subscriber subdis = n.subscribe("disturb", 6, disturb);
ros::Publisher pub = n.advertise<publisher::Position>("positions", 6);
//---------------END ROS SETUP---------------------------//
printf("Waiting for %d motor control nodes to connect to position publisher...\n", 1);
while(pub.getNumSubscribers() < 1)
{
usleep(100000);
}
printf("All nodes connected!\n");
while(1)
{
//Fetch current positions
getMotorValues();
publisher::Position position;
position.motor = 6;
position.pos = dxl6_present_position;
pub.publish(position);
//Subscribe torque
ros::spinOnce();
//Write torque
dmxWrite(2, ADDR_MOTOR[5], ADDR_MX_GOAL_TORQUE, torques[5]);
}
return 0;
}
The other node is the actual PID controller. It listens to a topic /positions and calculates what torque to apply, then publishes that on /torques.
void pidControlLoop()
{
//Setup ROS nh, publisher, subscriber
ros:: NodeHandle n6;
ros::Subscriber sub = n6.subscribe("positions", 6, updatePosition);
ros::Subscriber goalSub = n6.subscribe("goalpositions", 1000, updateGoalPosition);
ros::Publisher pub = n6.advertise<publisher::Position>("torques", 6);
ros::Rate rate(5);
publisher::Position torque_msg;
torque_msg.motor = MOTOR;
//INITIALIZE error values, error sums, gains
int pError = 0;
int iError = 0;
int dError = 0;
int errorSum= 0;
int errorOld= 0;
float torque;
float pk= 0.01;
float ik= 0.08;
float dk= 0.00;
while(position == -1)
{
ros::spinOnce();
}
pError = goalPosition - position;
errorOld = pError;
while(1){
pError = goalPosition - position;
//printf("pError: %d\n", pError);
if(abs(pError) > 50)
{
if(abs(pError) > 2048 && goalPosition > position)
{
pError = (goalPosition - 4096) - position;
printf("crossing ccw: %d\n", pError);
}
else if (abs(pError) > 2048 && goalPosition < position)
{
pError = (goalPosition + 4096) - position;
printf("crossing ccw: %d\n", pError);
}
errorSum += pError;
dError = pError - errorOld;
torque = pk*pError + dk*dError + ik*errorSum;
if(torque > maxtorque)
{
torque = maxtorque;
}
else if(torque < -maxtorque)
{
torque = -maxtorque;
}
//DMX torque: 0-1024 clockwise, 1024-2048 counterclockwise
if(torque < 0)
{
torque = -torque + 1024;
}
torque_msg.pos = torque;
}
else
{
torque_msg.pos = 0;
}
pub.publish(torque_msg);
ros::spinOnce();
rate.sleep();
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "motor6control");
pidControlLoop();
}
The PID controller functions. Obviously it is not well tuned, but after spending some time tuning by hand, some motors are able to get to the goal position, stay there, and respond to any disturbances. I am attempting to use the following diagram from help center as a model:
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1524106/image.png)
This is the Simulink model I've created:
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1524111/image.png)
Here are my steps during testing. I think the issue is likely related to a mistake I'm making here, as I'm pretty clueless about control design.
- Run both above ROS nodes, and make sure that the PID controller is adjusting torque as expected
- Click the Monitor and Tune button in Simulink
- Watch the ROS topics and see the test run
When I do this, a few weird things happen.
- The %conv almost immediately displays 100
- All 3 PID gains display 0
- Eventually, the autotuner starts writing to the disturb topic and adjusting the torques as expected. However, it will continue making seemingly random adjustments until the simulation reaches its stop time.
- All PID gains still read 0, even when the data is exported to MATLAB.
- Switching the start/stop signal off after the test starts does nothing.
Once the simulation ends, the motor will stop moving erratically, but no gains are ever displayed anywhere that I can find. Any help or advice is much appreciated.
6 Comments
Simone Cortinovis
on 6 Dec 2023
Since the topic covered in this question is more general, I have opened a new question at this link.
Arkadiy Turevskiy
on 12 Jan 2024
Thanks Simone for your analysis, we provided the answer in your stand-alone question.
On the topic of the original question: I wonder if using ROS nodes in this setup is introducing such significant delays into the setup that it creates a problem for the autotuner.
PID Autotuner works in two steps- first it estimate a frequency response of the plant at a few frequency points. Then it uses this frequency response to compute PID gains.
I suggest you try setting Experiment Mode to PRBS (this will use pseudo-random binary sequence as perturbation), which will hopefully produce frequency response faster. I also suggest adding optional outpit of plant frequency response - see if you get any non-zero values there.
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1589591/image.png)
Answers (0)
See Also
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!