# Closed loop PID control of a DC motor

After getting the DC motor to work nicely with my PS3 joystick, I continued by building in a control system using the PID algorithm. A feedback based control system is very important for advanced robotics, as it makes sure that what your software thinks is happening, actually does happen. A closed-loop feedback process works by taking in a sensor input and using it to adjust the output. In the case of my rather simple DC motor, an encoder generates ticks that are used to find the RPM of the motor, which is the original command.

#### Math

Briefly, here’s what the PID controller does, mathematically. The Wikipedia article above does a great job explaining the core components, so here is a translation of that main equation into practical pseudo code. $u(t) = K_{p}e_{n} + K_{i}\int e_n + K_{d}\frac{de_{n}}{dt}$

where $K_{p}$ = Proportional scale factor $e_{n}$ = input error (setpoint – input) $K_{i}$ = Integral scale factor $\int e_n$ = cumulative error (sum += error) $K_{d}$ = Derivative scale factor $de_{n}$ = change in error since last calculation $dt$ = change in time since last calculation

I turned this math into a C++ node for ROS that takes an RPM input and spins the motor to that correct speed. In the previous open loop behaviour, one node takes joystick input and outputs directly to the Arduino for control. After working on the next phase, I realized that would work better in more generic nodes, which is the more “ROS way” of doing it. So instead I have one node that takes the joystick input and outputs a left/right number scaled to some limit (in this case the maximum 58 RPM of the motors). A second node reads that RPM node output and uses a PID controller to generate motor output commands. This configuration makes it much easier to test because I can use standard ROS message publishing to set a target speed.

Here I’ll show and explain the different C++ code used to build the required nodes.

#### Generating RPM values

First is a node called motor_RPM_node that takes encoder ticks from the Arduino and publishes the current RPM speed.

class MotorRPM
{
public:
MotorRPM();

private:
void encoderCallback(const yardbot_msgs::LeftRightInt32::ConstPtr &msg);
float rpmFromEncoderCount(int count, double time);
void updateParameters();

int countPerRev;
ros::Time lastMessageTime;
yardbot_msgs::LeftRightInt32 lastEncMessage;
yardbot_msgs::LeftRightFloat32 lastRpmMessage;
ros::Subscriber encoderSub;
ros::Publisher rpmPub;
ros::NodeHandle nh, _nh;
};

MotorRPM::MotorRPM() {
// set the handler for the node's private namespace
_nh = ros::NodeHandle("~");

encoderSub = nh.subscribe("/arduino/encoder", 10, &MotorRPM::encoderCallback, this);

updateParameters();
}

void MotorRPM::updateParameters() {
// update the parameters for processing the joystick messages
if (!_nh.getParam("count_per_rev", countPerRev))
countPerRev = 720;
}

void MotorRPM::encoderCallback(const yardbot_msgs::LeftRightInt32::ConstPtr &msg) {
yardbot_msgs::LeftRightFloat32 rpm_msg;

// get the time difference
ros::Time now = ros::Time::now();
ros::Duration time = now - lastMessageTime;

int leftDifference = msg->left - lastEncMessage.left;
int rightDifference = msg->right - lastEncMessage.right;

rpm_msg.left = rpmFromEncoderCount(leftDifference, time.toSec());
rpm_msg.right = rpmFromEncoderCount(rightDifference, time.toSec());

// publish all the time
rpmPub.publish(rpm_msg);

lastMessageTime = now;

lastRpmMessage.left = rpm_msg.left;
lastRpmMessage.right = rpm_msg.right;

lastEncMessage.left = msg->left;
lastEncMessage.right = msg->right;
}

float MotorRPM::rpmFromEncoderCount(int count, double time) {
float rpm = (count/(float)countPerRev)/time*60;

return rpm;
}

int main(int argc, char** argv) {
ros::init(argc, argv, "motor_rpm_node");
MotorRPM motor_rpm_node;

ros::spin();

return 0;
}

This is a relatively simple node that subscribes to /arduino/encoder and publishes on /motors/rpm. You can see that it allows you to set the number of encoder ticks per revolutions with a launch parameter. The core method is the encoder callback, which builds the time since the last tick then uses the ticks per revolution value to generate the RPM and publish in left/right configuration using a custom YardBot message.

Since the joystick teleoperation node is pretty common, I’ll leave that out. Basically depending on joystick position, there is a stream of left/right messages published to /cmd_num with the desired RPM of each motor.

#### A PID class

Before explaining the main component, we need a PID class to handle generating valid output. This is an excellent time to use object oriented programming practices and build a PID class that can be reused.

class PID {
public:
PID();
PID(double _kp, double _ki, double _kd);
void setTarget(double _target);
void setInput(double _input);
void setTunings(double _kp, double _ki, double _kd);
void setOutputLimits(double _min, double _max);
double compute();
double getKp();
double getKi();
double getKd();
double getTarget();
double getInput();
void setActive(bool _active);

private:
void reset();
double kp, ki, kd;
double input, target, output;
double ITotal, prevInput, prevError;
double minLimit, maxLimit;
bool active;
ros::Time prevTime;
};

PID::PID(double _kp, double _ki, double _kd) {
kp = _kp;
ki = _ki;
kd = _kd;
reset();
output = 0;
input = 0;

setOutputLimits(0, 255);

//ROS_DEBUG("Kp %.2f; Ki %.2f; Kd %.2f", kp, ki, kd);
}

PID::PID() {
reset();
}

void PID::setTarget(double _target) {
target = _target;
}

void PID::setInput(double _input) {
input = _input;
}

void PID::setTunings(double _kp, double _ki, double _kd) {
kp = _kp;
ki = _ki;
kd = _kd;

reset();
};

void PID::setOutputLimits(double _min, double _max) {
if (_min > _max) return;

minLimit = _min;
maxLimit = _max;
}

double PID::compute() {
ros::Time now = ros::Time::now();
ros::Duration change = now - prevTime;

double error = target - input;
ITotal += error * ki;
ITotal = std::min(ITotal, maxLimit);
ITotal = std::max(ITotal, minLimit);
double dValue = kd * (error - prevError)/change.toSec();

/* do the full calculation */
output = kp * error + ITotal + dValue;

/* clamp output to bounds */
output = std::min(output, maxLimit);
output = std::max(output, minLimit);

/* required values for next round */
prevTime = now;
prevInput = input;

/* debug some PID settings */
//ROS_DEBUG("P %.2f; I %.2f; D %.2f", error, ITotal, dValue);

return output;
};

double PID::getKp() {
return kp;
}

double PID::getKi() {
return ki;
}

double PID::getKd() {
return kd;
}

double PID::getTarget() {
return target;
}

double PID::getInput() {
return input;
}

void PID::setActive(bool _active) {
if (!active && _active)
reset();
active = _active;
};

void PID::reset() {
ITotal = output;
prevInput = 0;
prevError = 0;

ITotal = std::min(ITotal, maxLimit);
ITotal = std::max(ITotal, minLimit);
};

Much of the overall process and procedure was pulled from the excellent PID library for Arduino by Brett Beauregard. There are many convenience methods for retrieving data about the controller and it shows the input data contributing to the error. Once created as an object, the main controller simply needs to mark a setpoint, set the actual input and receive the processed output.

#### The main controller

Finally comes the main controller node. It subscribes to the encoder RPM topic and RPM setpoint topic and publishes to the Arduino motor. This is a pretty standard ROS paradigm: take input, process and generate output.

class MotorPID {
public:
MotorPID();

private:
void numberCallback(const yardbot_msgs::LeftRightFloat32::ConstPtr &msg);
void encoderCallback(const yardbot_msgs::LeftRightFloat32::ConstPtr &msg);
bool setTuningsServiceCallback(yardbot_msgs::Tunings::Request &req,
yardbot_msgs::Tunings::Response &res);
void updateParameters();
void controllerTimerCallback(const ros::TimerEvent& e);
float constrain(float value, float min, float max);
int mapRpmToOutput(float rpm);

ros::Subscriber numberSub, encoderSub;
ros::Publisher motorPub;
ros::ServiceServer tuningService;

ros::NodeHandle nh, _nh;
ros::Timer controllerTimer;

PID leftRPMController, rightRPMController;
double kp, ki, kd;
yardbot_msgs::LeftRightInt32 lastMotorMsg;

int maxRpm, maxPwmOutput;
float prevLeftMotor, prevRightMotor;
};

MotorPID::MotorPID() {
// set the handler for the node's private namespace
_nh = ros::NodeHandle("~");

maxRpm = 58;
maxPwmOutput = 127;

numberSub = nh.subscribe("/cmd_num", 10, &MotorPID::numberCallback, this);
encoderSub = nh.subscribe("/motors/rpm", 10, &MotorPID::encoderCallback, this);
controllerTimer = nh.createTimer(ros::Duration(0.2), &MotorPID::controllerTimerCallback, this, false);

updateParameters();

leftRPMController = PID(kp, ki, kd);
rightRPMController = PID(kp, ki, kd);
}

void MotorPID::updateParameters() {
if (!_nh.getParam("kp", kp))
kp = 0.1;

if (!_nh.getParam("ki", ki))
ki = 0;

if (!_nh.getParam("kd", kd))
kd = 0;
}

bool MotorPID::setTuningsServiceCallback(yardbot_msgs::Tunings::Request &req, yardbot_msgs::Tunings::Response &res) {
leftRPMController.setTunings(req.kp, req.ki, req.kd);
rightRPMController.setTunings(req.kp, req.ki, req.kd);

return true;
}

void MotorPID::encoderCallback(const yardbot_msgs::LeftRightFloat32::ConstPtr &msg) {
leftRPMController.setInput(msg->left);
rightRPMController.setInput(msg->right);
}

void MotorPID::numberCallback(const yardbot_msgs::LeftRightFloat32::ConstPtr &msg) {
// take the target RPM and set the PID target speed
if (msg->left != prevLeftMotor || msg->right != prevRightMotor) {
leftRPMController.setTarget(msg->left);
rightRPMController.setTarget(msg->right);

// ROS_INFO("targets %.2f %.2f", leftMotor, rightMotor);
}
}

float MotorPID::constrain(float value, float min, float max) {
value = std::min(value, max);
value = std::max(value, min);

return value;
}

int MotorPID::mapRpmToOutput(float rpm) {
// convert from a desired RPM value to a motor command (0 - 255)

// from http://arduino.cc/en/reference/map
int in_min = -maxRpm, in_max = maxRpm;
int out_min = -maxPwmOutput, out_max = maxPwmOutput;
return (rpm - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void MotorPID::controllerTimerCallback(const ros::TimerEvent& e) {
yardbot_msgs::LeftRightInt32 motorMsg;

double leftMotor = leftRPMController.compute();
double rightMotor = rightRPMController.compute();

// handle the zero case
if (leftRPMController.getTarget() == 0 && leftRPMController.getInput() == 0)
leftMotor = 0;

if (rightRPMController.getTarget() == 0 && rightRPMController.getInput() == 0)
rightMotor = 0;

motorMsg.left = mapRpmToOutput(leftMotor);
motorMsg.right = mapRpmToOutput(rightMotor);

if (motorMsg.left != lastMotorMsg.left || motorMsg.right != lastMotorMsg.right) {
motorPub.publish(motorMsg);
ROS_INFO("PID computed %.2f %.2f; target %.2f %.2f; input %.2f %.2f",
leftMotor, rightMotor,
leftRPMController.getTarget(), rightRPMController.getTarget(),
leftRPMController.getInput(), rightRPMController.getInput()
);
}

lastMotorMsg.left = motorMsg.left;
lastMotorMsg.right = motorMsg.right;
}

int main(int argc, char** argv) {
ros::init(argc, argv, "motor_pid_node");
MotorPID motor_pid_node;

ros::spin();

return 0;
}

Once again there is a definition of the class, along with the different ROS callbacks. There is a subscriber for the RPM input and encoder RPM data, plus a service for setting the tuning parameters. I use a launch file to set the different nodes and parameters, but while testing it becomes cumbersome to constantly restart the launch file to use the updated values. By having a service server, I can set the motor to 0 and update the tunings on the fly, then mark them in the launch file.

As before the initialization methods create the callback objects and receive the launch parameters. The tuning callback sets the PID controller values. The encoder callback sets the PID input values and the RPM callback sets the PID target.

The main processing happens inside a timer callback function that is run every 0.2 seconds. This timing requires taking different factors into consideration: processing speed of your computer, output response requirement and the capabilities of your output (DC motor in this case). My particular number may change as I learn more about the overall system.

Inside the timer callback, the PID controllers generate the next output. For the special zero case, the motors are also turned off. The node tracks the output values and only publishes if the desired speed changes. This is to reduce the number of messages being sent to the Arduino. The PID controller outputs in RPM values but the motor controller itself requires an integer between -127 and 127 so there’s an extra step where the RPM value is mapped to the motor controller parameter.

#### Tuning the controller

So now that the controller is set up, the most time consuming aspect of building the control system is tuning it to safe and efficient levels. To do this, you need to be able to interpret response characteristics as tuning parameter changes. Admittedly I’m still working on that part, but there’s enough info on the internet that I was able to see progress. In order to see how the controller was behaviour, I opened ROS on my desktop and used rqt_plot to display the desired RPM value against the actual RPM output. That’s what I used for the nice graph at the top. First is output with a large overshoot. I increased Ki a little more. Here Kp is too low, as the output doesn’t overshoot the target of 40 RPM. This also gives a good example of what control systems are supposed to do: eliminate error. Given enough time, this system would still reach the target. With this graph, I changed the target multiple times. You can see the response becomes much smoother as time goes on. Ideally all responses would be smooth like that, so I’ll continue to look into those improvements. Finally I hooked my joystick node back in and used it to move the motor around. Here you can clearly see how the output follows the target. There are a few uneven spots where I can continue to optimize but it’s very clear that the basic behaviour is there. The next phase of improving the controller is to add some safety features that will monitor the output so that it behaves correctly in all scenarios.

Tagged with: , ,
###### 2 comments on “Closed loop PID control of a DC motor”
1. sunnywinter says:

When I use rosserial subscribe or publish a topic,belong the node serial_node.How can I publish to arduino motors? or I use CMAKE to make a file using cpp code? And can i see you full arduino code?

• Wes says:

You’ll need to write code for the Arduino to subscribe to the published motor commands. Is that something you’ve done yet? Basically you publish messages like left = 128, right = 100 and interpret that with the Arduino.