# PID algorithm implementation using computer vision

I am constructing an automatic labyrinth maze solver, and using a webcam to control my maze.

Based on suggestions in other forums, I am trying to control maze's ball movement at least in one direction at the moment. So, I am trying to control my ball movement between two coordinates 466,288 and 466,152. The input to the stepper motor controller board is time, no of steps to rotate for each axis i.e x and y.

The stepper motor controller board that I am using is the egg bot stepper motor controller board: http://www.sparkfun.com/products/10025

So in order to move between two points, should I create a number of way points between the two points namely 288 and 152 (say 260 240 230 ... 150) and correct my ball movement?

My image processing algorithm is not fast enough to track the ball that the ball would just spin and fall into a hole.

Some suggested that I use a standard template as shown in the following video and correct my ball movements for deviation in the path:

I also came across an image processing tool where they solved the same problem using way points for the ball movement. Seeing too many solutions to the same problem, I am totally confused in going about solving the problem. I am aware that I should implement a PID controller. But how should I go about solving the problems in stages? I am stuck and just frustrated in finding a headstart in solving the problem.

My setup looks like this:

...and here's a screenshot of my software:

Revision 2: I am also facing a new problem now: Earlier I controlled the stepper motors via Arduino serial port Java applet. I am able to drive the steppers using the applet.

I have to reset the board every time I try to communicate via serial port. Also, the stepper motor energizes itself in small intervals when no command is sent to it. When the stepper motor enters this mode, I cannot control my board without resetting the board. Any assistance would be appreciated.

Revision 3:

I made some progress where I got the PID algorithm implemented. Please find the video below: http://www.youtube.com/watch?v=MEfp7RqPmqY

Now I have a problem with the speed at which the PID algorithm is implemented. Actually my image processing finishes a cycle in 200 ms, identifies a ball and send the commands to the stepper motor controller board. Even though my serial port is sent commands to switch directions, my stepper keeps rotating in the same direction. You can find the weird behavior in the video above.

My thought is that I should restrict the PID values with a ceiling where if the calculated PID value is greater than 100, I should just send a 100. I am looking forward to hear your thoughts on this.

The way I implemented the PID controller is that I identified the starting point of the template using the template matching algorithm and identified the ball using another template matching algorithm. Now, I made the ball move to the centroid of the starting point template. How do I make it follow the straight line with the PID algorithm?

Revision 4:

I have isolated the trajectory but I am not able to find the correct function to print the correct pixel coordinates from the starting point. Any thoughts?

• Re PS: Then just post a link to the image and I'm sure someone will come along and replace it with the image itself... Commented Jun 29, 2011 at 10:52
• @Matt - Fixed! However, I'd prefer that the user provided the text to go with the images, not just the links to the images. I'm not sure where @Sai wanted them, I just put them at the bottom. Commented Jun 29, 2011 at 19:20
• Wow.... Do those steppers have anywhere enough torque to move the board with any speed? I hope there is some gear reduction in there somewhere. Commented Jun 30, 2011 at 5:17
• @Fake - The steppers should have no problem with this. The board doesn't weigh much and its weight is balanced. I have a wall clock with hands 40cm long and it's controlled by the same small mechanism as any other, that's also a stepper. (The 5cmx5cm mechanism looks ridiculously small compared to the 80cm diameter of the clock) Commented Jun 30, 2011 at 6:49
• @Fake: Steve is right. I do not have problem with the stepper. It is all about the PID algorithm
– Sai
Commented Jul 1, 2011 at 1:18

First, since steppers are great at positioning (there is no need for a position feedback), you should certainly limit their movement as you've said yourself. I am not sure how the motor shaft is engineered right now, but if it was fixed to the motor, letting it continue spinning would risk damaging the equipment.

Next, 200ms transport delay in your sensor will probably be too slow, otherwise you will need to slow things down a lot in order to slow down the ball itself. Similar to what Rocket Surgeon said, you should simplify the image processing algorithm to calculate the path only once, and then quickly calculate only the position of the ball in each frame. If you want to skip this step quickly, find a red ball instead of this one, and then check only the red component in your RGB picture, until you've found a better algorithm.

For the PID control, start with the fact that you actually need two separate PID controllers, one for the east-west motor, the other one for the north-south one. If you have two exact motors, their parameters must be equal.

For a PID controller to act, it needs to know the error: difference between the desired position, and the actual position of the ball. X and Y components of this offset will be the inputs for two PID controllers (one for each motor). To get the error, you need to have the desired position on your path first: a trajectory.

To get the trajectory, you need to process the image and get the path, as well as its starting and ending point. I am not sure if your algorithm is capable of distinguishing the path from the rest of the board right now, but if not, note that this is an algorithm of its own to handle before continuing. Again, you can skip this part by manually entering the junction points, if you are eager to see some results quickly. In any case, you should be able to define the setpoint speed, and have your software move the desired coordinate position over the path, from start towards the end. Obviously, you will start with a low desired speed.

So, before starting with control, you should go through the following checklist first:

• Simplify your image processing algorithm to get faster response
• Create an algorithm which creates a trajectory over your path using a predefined speed
• In each frame:
• Calculate the difference between the trajectory and the ball position
• Pass the delta-X component to the east-west PID, pass the delta-Y to the north-south PID

It may turn out that it is better to create the trajectory one segment at a time, and continue with the next segment when that ball ends the previous one. Otherwise, you will need to take care that the ball doesn't overshoot the desired trajectory (which may be hard to accomplish)

If you have a fixed position for camera and mometary reading for both axes, then you dont need to recognize painted path, holes and walls in frames. It can be done one-shot during setup time. In run-time you may only need to spot exact location of the single shiny metal ball.

To spot the ball you can use 1 fixed spot IR LED and narrowband filter on camera. The algorithm must calculate the brightest pixel and translate X,Y to real X,Y taking into account steps like:

• find brightest pixel
• use angle for both axes (reading from servo) to recover the distance from camera
• use timestamp for axes position reading
• apply interpolation through time for position reading if needed
• use known distortion of lens
• translate world X,Y of pixel to angle of reflection from perfect sphere to find true center of ball in world X,Y
• delta time to recover true time of frame
• interpolation in time of position in board plane X,Y if needed
• send result X,Y(t) to PID algorithm
• send second feed goal X,Y(t) from trajectory generator/sequence
• let PID to decide on output
• execute output (last steps can be done in parallel)

It should not be computationally intensive and mostly depends on couple of absolute values.

Normally, small CPU should do it with the pace of framerate.

• I am not sure that I understand your solution. I find your solution interesting. How do I ensure that my ball is following the correct path? Should I ensure that I have a set of waypoints to follow?
– Sai
Commented Jul 1, 2011 at 1:30
• Yes. The motion control software must always have a "trajectory generator", which is a routine producing ideal finite sequence of X,Y (t) for any given step in time. This sequence is fed to first input of control loop, second input of control loop is sequence of real positions. Control routine must calculate position/velocity/acceleration errors and amplify/summate all errors accordingly to PID and produce resulting correcting signals.
– user924
Commented Jul 1, 2011 at 13:19