# How to program parallel PID control loops? So I can give my robot multiple set points to follow

So I'm in the process of building my robot and it has encoders on every wheel measuring speed and position and a compass sensor measuring heading.

I have 3 seperate PID loops at the moment, I can either control the robots speed or I can control the robots position or I can make it follow a heading using a line following type algorithm.

Now I am completely stuck on how to merge these control loops, how do I control the robots heading AND its speed. So I can say to it, "go at 20 degrees at 3m/s" or "go at 45degrees for 5 metres then stop"

Really I would like to be able to control all 3, heading speed and position so I could say "go at 20 degrees for 10 metres at a speed of 5m/s" however I have no idea how to merge the loops.

At the end of the day there are 3 inputs (heading, speed and position) and 1 output (PWM to motors) so do I need to use some kind of MISO control scheme, if so which one?

I've looked at cascaded control but that only accepts 1 set point where I want to set 3 different set points. Maybe some kind of state machine? I'm so stuck!!

What you need to do is make a LAYERED control system. At the bottom layer is the motor speed PID controller. It will constantly read two parameters "target speed" and "count to stop at". There is one of these for each wheel. The Wheel-PID runs the wheel at the desired speed until the encoder count matchers the stop count.

Next you have a heading PID. It's output is the "target speed" for each of the wheels. It it needs to go more to the left it increases the speed of the right wheels and reduces the left wheel speed. This servos the robot to the desired heading.

The next level outputs the desired heading and the "stop at" encoder count. It knows what heading to go to in order to get to the desired location and how to avoid obstacles and it can set the target speed based on maybe how close the robot is to the location. It get the current location be integrating the path actually traveled.

You were right when you said "cascade". One PID's output is the next one's input. But it need not be one to one

But as you move upward perhaps "PID" is not the right method?

Also you will find you need to combine sensor data when two sensors give the same information like wheel encodes and a compass. You can use a Kalman filter to combine this data.

• Thankyou for posting this, it explains what I need to do perfectly. Now to just get it working! – lilSebastian Feb 8 '15 at 20:55

Try to gather some basic knowledge on the argument by reading papers about the control of the unicycle; here's one of them. This article has pretty much everything you need.

A somehow related question.

• Great reference. – Spiked3 Jan 2 '15 at 4:52

As you noted, there is only one output (the motor's desired PWM) when all is said and done. All three loops may contribute to this output, but how should this be managed? The simplest method is to average all 3 commanded inputs to form a single PWM value, $PWM=(heading+speed+position)/3$.

Of course, this method assumes that all command forms are equally valued. Is heading more important? Then weight it more: $(alpha * heading)+(beta * speed)+(gamma * position)$, where $alpha+beta+gamma = 1$ and $alpha > beta$ and $alpha > gamma$.

• Ahh right, is this also called a complementary filter? As I have seen something similar in some IMU accelerometer/Gyroscope software. – lilSebastian Feb 8 '15 at 20:56