raspberry pi color tracking using pid - oscarliang
TRANSCRIPT
Log in (http://blog.oscarliang.net/wp-login.php)
Register (http://blog.oscarliang.net/wp-login.php?action=register)← Build A Quadcopter From Scratch – Hardware Overview
(http://blog.oscarliang.net/build-a-quadcopter-beginners-tutorial-1/)
Fun Projects → (http://blog.oscarliang.net/fun-projects-1/)
Raspberry Pi Color TrackingUsing PIDPosted on June 30, 2013 (http://blog.oscarliang.net/raspberry-pi-color-tracking-opencv-pid/) by Oscar
(http://blog.oscarliang.net/author/oscar/)
Project Introduction
In this project I implemented OpenCV color recognition on the Raspberry Pi
(http://blog.oscarliang.net/tag/raspberry-pi/) that uses PID to control the pan-tilt servo
system. In this post, I will explain briefly how color tracking works, and how to use PID
control algorithm to improve tracking performance. Like my previous face recognition
tutorial (http://blog.oscarliang.net/raspberry-pi-face-recognition-opencv/), I will be using
the Wall-E robot (http://blog.oscarliang.net/wall-e-robot-in-real-life/) in this Raspberry Pi
Color Tracking project as an example.
Raspberry Pi Color Tracking Using PID - OscarLiang.net http://blog.oscarliang.net/raspberry-pi-color-tracking-opencv-pid/
1 of 11 1/23/2014 12:25 PM
The Raspberry Pi has relatively small computational capacity compared to a laptop or PC.
Because of that you might notice the sluggish result in face recognition. But in color
tracking, the result is quite smooth and satisfactory because the computational process is
not as complex as face recognition.
We can use Raspberry Pi to control the servos directly using interface like ServoBlaster
(https://github.com/richardghirst/PiBits/tree/master/ServoBlaster). But instead, I use the
Arduino (http://blog.oscarliang.net/tag/arduino/) as a servo controller. Not only it’s easier to
manage, but also there is PID library (http://playground.arduino.cc/Code/PIDLibrary)
already available as a library on Arduino (http://arduino.cc/en/).
So this is how it works: the Raspberry Pi detects the color, work out the coordinates and
send to the Arduino via I2C (http://blog.oscarliang.net/raspberry-pi-arduino-connected-
i2c/). The Arduino will then feed the data to the PID controller to calculate how much and
to which direction to turn the servos.
Raspberry Pi Color Tracking Using PID - OscarLiang.net http://blog.oscarliang.net/raspberry-pi-color-tracking-opencv-pid/
2 of 11 1/23/2014 12:25 PM
What is PID and How to Use It?
PID is a closed loop control system that is trying to minimize the error. In simple words, it
takes the inputs from your sensor, and you tell it what your target set-point is, and it will
come up with a output adjustment which aims to help your system to get closer to the
set-point.
PID closed loop controller
Raspberry Pi Color Tracking Using PID - OscarLiang.net http://blog.oscarliang.net/raspberry-pi-color-tracking-opencv-pid/
3 of 11 1/23/2014 12:25 PM
To understand more about the PID control system, I found this basic PID tutorial
(http://robot-kingdom.com/pid-controller-tutorial-for-robots/) very useful.
The most important factor of a optimal PID controller is its three constant parameters. To
achieve good performance, we need to play around with different values to see different
results. I will also talk about some tuning practices that I found helpful.
How to Use Arduino PID Controller
Using PID on Arduino is very easy, simply follow the instructions on this page
(https://github.com/br3ttb/Arduino-PID-Library/) to setup the library and we are good to go!
Following the examples provided in the library, its not hard to see how to use it. Basically,
we need to:
include the library
define the necessary variables
Construct the PID controller and establish links with the variables, specify the tuning
constants
Measure input and feed into the PID controller, and retrieve output
Tips on Tuning the PID constants
Depending on how you are using the output to adjust your system, the PID constants
parameters will be different, so make sure have settled down on that before you start
tuning. Sometimes not all three constants are needed (can be zero), it’s all down to your
requirements and performance. If you don’t think some of the constants are helping, then
set it to zero.
To start with, I usually set all PID constant parameters to 0, and then tune each constant in
order, then randomly fine tune each one.
P (proportional), the key here is to get a quick strong response without any shake or
#include <PID_v1.h>
double Setpoint, Input, Output;
PID myPID(&Input, &Output, &Setpoint, 0.4, 0.4, 0, DIRECT);
Input = meansurement;
myPID.Compute();
Adjustment = Output;
Raspberry Pi Color Tracking Using PID - OscarLiang.net http://blog.oscarliang.net/raspberry-pi-color-tracking-opencv-pid/
4 of 11 1/23/2014 12:25 PM
vibration. From a small number and work your way up. The error rate at this point will be
high and final accurate leveling will be slow. When this is set too high you will produce a
high speed shake.
I (integral). The Integral algorithm will add more and more to the corrective action. This
can help to balance inherent inconsistencies to the system, smoothing out errors over
time. When this is set too high it will produce a slow wobble or oscillation (overshoot?),
when it’s set too low errors will occur (damping effect?).
D (derivative). This parameter can have positive effect on overall stability of a mechanical
system, i.e. it helps to overcome the inertia faster, etc.
Raspberry Pi Color Tracking and Source Code
Color Tracking using OpenCV is really simple, We basically need to go through this steps
on the Raspberry Pi every time.
Capture Image
Throw away the pixels which are not falling in the range and high-light the pixel
which are in the range, so you will see a black image with white dots and puddles.
When the detected color has a large enough area, calculate the center position of
that color using image moments.
Send the position off to.
In the source code, we can choose whatever color you want to track. Look for the
InRangeS() (http://docs.opencv.org/modules/core/doc
/operations_on_arrays.html#cv.InRangeS) function. It takes source, lower
bound color, upper bound color and destination. Just replace the lower bound and upper
bound colors with the HSV values.
You can find this value of your favorite color using Gimp or MS Paint. Note that software
like Gimp and MS paint use Hue value ranging from 0-360, Saturation and Value from
0-100%. But OpenCV uses 0-180 for Hue and 0-255 for Saturation and Value. So you
need to do some conversion before plugging the values in.
1 # Raspbery Pi Color Tracking Project2 # Code written by Oscar Liang3 # 30 Jun 20134 5 import cv2.cv as cv6 import smbus7 8 bus = smbus.SMBus(1)9 address = 0x04
10
Raspberry Pi Color Tracking Using PID - OscarLiang.net http://blog.oscarliang.net/raspberry-pi-color-tracking-opencv-pid/
5 of 11 1/23/2014 12:25 PM
11 def sendData(value):12 bus.write_byte(address, value)13 # bus.write_byte_data(address, 0, value)14 return -115 16 def readData():17 state = bus.read_byte(address)18 # number = bus.read_byte_data(address, 1)19 return state20 21 def ColorProcess(img):22 23 # returns thresholded image24 imgHSV = cv.CreateImage(cv.GetSize(img), 8, 3)25 26 # converts BGR image to HSV27 cv.CvtColor(img, imgHSV, cv.CV_BGR2HSV)28 imgProcessed = cv.CreateImage(cv.GetSize(img), 8, 1)29 30 # converts the pixel values lying within the range to 255 and stores it in
the destination31 cv.InRangeS(imgHSV, (100, 94, 84), (109, 171, 143), imgProcessed)32 return imgProcessed33 34 def main():35 36 # captured image size, change to whatever you want37 width = 32038 height = 24039 40 capture = cv.CreateCameraCapture(0)41 42 # Over-write default captured image size43 cv.SetCaptureProperty(capture,cv.CV_CAP_PROP_FRAME_WIDTH,width)44 cv.SetCaptureProperty(capture,cv.CV_CAP_PROP_FRAME_HEIGHT,height)45 46 cv.NamedWindow( "output", 1 )47 cv.NamedWindow( "processed", 1 )48 49 while True:50 51 frame = cv.QueryFrame(capture)52 cv.Smooth(frame, frame, cv.CV_BLUR, 3)53 54 imgColorProcessed = ColorProcess(frame)55 mat = cv.GetMat(imgColorProcessed)56 57 # Calculating the moments58 moments = cv.Moments(mat, 0)59 area = cv.GetCentralMoment(moments, 0, 0)60 moment10 = cv.GetSpatialMoment(moments, 1, 0)61 moment01 = cv.GetSpatialMoment(moments, 0,1)62 63 # Finding a big enough blob64 if(area > 60000):65 66 # Calculating the center postition of the blob67 posX = int(moment10 / area)68 posY = int(moment01 / area)69 70 # check slave status and send coordinates71 state = readData()72 if state == 1:73 sendData(posX)74 sendData(posY)75 print 'x: ' + str(posX) + ' y: ' + str(posY)76 77 # update video windows78 cv.ShowImage("processed", imgColorProcessed)79 cv.ShowImage("output", frame)80
Raspberry Pi Color Tracking Using PID - OscarLiang.net http://blog.oscarliang.net/raspberry-pi-color-tracking-opencv-pid/
6 of 11 1/23/2014 12:25 PM
Sourcecode on The Arduino
The code on the Arduino is mainly about how the servos are controlled by PID and how to
use PID. I think I have explained pretty much everything here in the “How PID works in
Arduino” section.
One thing I noticed about Arduino PID library is that you have to explicitly specify both
negative and positive range of the output like this: myPIDX.SetOutputLimits(-255,
255); Otherwise if the output will only be positive, which means in our case, the servo will
only be told to turn right and not the other way.
Last note is on the variable called ‘status’. It will be set to 0 when we are receiving and
executing the command from raspberry pi, tell the pi we are busy and not ready for the
next command yet. This is because Color tracking is so fast and the commands are sent
more frequent than the Arduino can handle, which the problem we didn’t have in face
recognition.
81 if cv.WaitKey(10) >= 0:82 break83 84 return;85 86 if __name__ == "__main__":87 main()
1 // Raspbery Pi Color Tracking Project2 // Code written by Oscar Liang3 // 30 Jun 20134 5 #include <Wire.h>6 #include <Servo.h>7 #include <PID_v1.h>8 9 #define SLAVE_ADDRESS 0x04
10 11 #define NUM_DATA 212 byte data[NUM_DATA];13 byte cur_data_index;14 15 byte state;16 17 Servo servoNeckX;18 Servo servoNeckY;19 20 const byte servoNeckX_pin = 3;21 const byte servoNeckY_pin = 4;22 23 // Servo Angle constrains (Good Practice! <img src="http://blog.oscarliang.net
/wp-includes/images/smilies/icon_smile.gif (http://blog.oscarliang.net/wp-includes/images/smilies/icon_smile.gif)" alt=":)" class="wp-smiley">
24 const int lrServoMax = 2300; // looking right25 const int lrServoMin = 700;26 const int udServoMax = 2100; // looking down27 const int udServoMin = 750; // looking up28 29 // Init Servo Position30 int posX = 1500;
Raspberry Pi Color Tracking Using PID - OscarLiang.net http://blog.oscarliang.net/raspberry-pi-color-tracking-opencv-pid/
7 of 11 1/23/2014 12:25 PM
31 int posY = 1300;32 33 // --- Init PID Controller ---34 35 //Define Variables we'll be connecting to36 double SetpointX, InputX, OutputX;37 double SetpointY, InputY, OutputY;38 39 //Specify the links and initial tuning parameters40 // face tracking: 0.8, 0.6, 041 // color tracking: 0.4, 0.4, 042 PID myPIDX(&InputX, &OutputX, &SetpointX, 0.4, 0.4, 0, DIRECT);43 PID myPIDY(&InputY, &OutputY, &SetpointY, 0.4, 0.4, 0, DIRECT);44 45 void setup() {46 47 // --- I2C Setup ---48 49 // initialize i2c as slave50 Wire.begin(SLAVE_ADDRESS);51 52 // define callbacks for i2c communication53 Wire.onReceive(receiveData);54 Wire.onRequest(sendData);55 56 // --- Setup PID ---57 58 SetpointX = 100;59 SetpointY = 70;60 myPIDX.SetOutputLimits(-255, 255);61 myPIDY.SetOutputLimits(-255, 255);62 63 //turn PIDs on64 myPIDX.SetMode(AUTOMATIC);65 myPIDY.SetMode(AUTOMATIC);66 67 // --- Setup Servos ---68 69 servoNeckX.attach(servoNeckX_pin);70 servoNeckX.writeMicroseconds(posX);71 72 servoNeckY.attach(servoNeckY_pin);73 servoNeckY.writeMicroseconds(posY);74 75 state = 1;76 cur_data_index = 0;77 78 Serial.begin(9600); // start serial for output79 Serial.println("Ready!");80 81 }82 83 void loop() {84 delay(20);85 }86 87 // callback for received data88 void receiveData(int byteCount){89 90 // Update Slave Status - Occupied91 state = 0;92 93 while(Wire.available()) {94 95 data[cur_data_index++] = Wire.read();96 97 // When we have received both X and Y coordinates98 if(cur_data_index >= NUM_DATA){99 cur_data_index = 0;
100 101 // Calculate PID outputs with Inputs
Raspberry Pi Color Tracking Using PID - OscarLiang.net http://blog.oscarliang.net/raspberry-pi-color-tracking-opencv-pid/
8 of 11 1/23/2014 12:25 PM
Conclusion
Hope you enjoyed this post and found it helpful :-). Leave me a comment if you have any
suggestion or question.
If you find this article useful.
To help us maintain and improve this website.
Related Articles:
Raspberry Pi
Face
(http://blog.oscarliang.net
/raspberry-
pi-face-
recognition-
opencv/)
Raspberry Pi
and Arduino
(http://blog.oscarliang.net
/raspberry-
pi-arduino-
connected-i2c/)
Connect
Raspberry Pi
(http://blog.oscarliang.net
/connect-
raspberry-pi-and-
arduino-
usb-cable/)
Raspberry Pi
and Arduino
(http://blog.oscarliang.net
/raspberry-pi-and-
arduino-
connected-serial-
gpio/)
This entry was posted in Featured (http://blog.oscarliang.net/category/featured/), Raspberry Pi
(http://blog.oscarliang.net/category/raspberry-pi-2/), Robot (http://blog.oscarliang.net/category/robot/) and tagged
arduino (http://blog.oscarliang.net/tag/arduino/), raspberry pi (http://blog.oscarliang.net/tag/raspberry-pi/).
Bookmark the permalink (http://blog.oscarliang.net/raspberry-pi-color-tracking-opencv-pid/).
102 InputX = data[0];103 InputY = data[1];104 myPIDX.Compute();105 myPIDY.Compute();106 107 // Update Servo Position108 posX = constrain(posX + OutputX, lrServoMin, lrServoMax);109 posY = constrain(posY - OutputY, udServoMin, udServoMax);110 servoNeckX.writeMicroseconds(posX);111 servoNeckY.writeMicroseconds(posY);112 113 // Update Slave Status - Available114 state = 1;115 116 }117 118 }119 }120 121 // callback for sending data122 void sendData(){123 Wire.write(state);124 }
Raspberry Pi Color Tracking Using PID - OscarLiang.net http://blog.oscarliang.net/raspberry-pi-color-tracking-opencv-pid/
9 of 11 1/23/2014 12:25 PM
← Build A Quadcopter From Scratch – Hardware Overview
(http://blog.oscarliang.net/build-a-quadcopter-beginners-tutorial-1/)
Fun Projects → (http://blog.oscarliang.net/fun-projects-1/)
5 thoughts on “Raspberry Pi Color Tracking Using PID”
Raspberry Pi Face Recognition Using OpenCV - OscarLiang.net (http://blog.oscarliang.net
/raspberry-pi-face-recognition-opencv/) says:
June 30, 2013 at 11:37 pm (http://blog.oscarliang.net/raspberry-pi-color-tracking-opencv-pid/#comment-254)
[...] Here is the Arduino code. Note that this code uses a very dummy and basic open loop control method, I
only use this because of its simplicity. For a more optimal control method, please see Color Tracking Using
PID. [...]
Reply (/raspberry-pi-color-tracking-opencv-pid/?replytocom=254#respond)
Jason M says:
July 3, 2013 at 8:12 pm (http://blog.oscarliang.net/raspberry-pi-color-tracking-opencv-pid/#comment-275)
Very Useful, thank!
Reply (/raspberry-pi-color-tracking-opencv-pid/?replytocom=275#respond)
Raspberry PI urmareste culoarea | Robofun Blog (http://robofun.ro/blog/raspberry-pi-urmareste-
culoarea/) says:
August 12, 2013 at 8:02 am (http://blog.oscarliang.net/raspberry-pi-color-tracking-opencv-pid/#comment-516)
[...] Detalii complete. [...]
Reply (/raspberry-pi-color-tracking-opencv-pid/?replytocom=516#respond)
nutt_Thailand says:
January 15, 2014 at 3:24 am (http://blog.oscarliang.net/raspberry-pi-color-tracking-opencv-pid/#comment-1220)
Sorry, I’m a rookie I want to know how to connect wire between raspi – arduino – servo .
Thanks, for this : )
Reply (/raspberry-pi-color-tracking-opencv-pid/?replytocom=1220#respond)
Raspberry Pi Color Tracking Using PID - OscarLiang.net http://blog.oscarliang.net/raspberry-pi-color-tracking-opencv-pid/
10 of 11 1/23/2014 12:25 PM
Leave a Reply
Oscar (http://OscarLiang.net) says:
January 15, 2014 at 11:43 am (http://blog.oscarliang.net/raspberry-pi-color-tracking-opencv-pid/#comment-1226)
Servo is connected to the Arduino, like described here (http://blog.oscarliang.net/how-to-control-a-servo/)
Arduino is connected to the RPi via I2C, just like described here (http://blog.oscarliang.net/raspberry-
pi-arduino-connected-i2c/).
Reply (/raspberry-pi-color-tracking-opencv-pid/?replytocom=1226#respond)
Raspberry Pi Color Tracking Using PID - OscarLiang.net http://blog.oscarliang.net/raspberry-pi-color-tracking-opencv-pid/
11 of 11 1/23/2014 12:25 PM