This was a 'just for fun' project during exam time. It's a C program, using the opensource opencv computer vision library, that looks for blue thimbles, or fingerpupets, (but actually blue electrcal tape) on one's fingers, and uses the information to control a robotic arm through the serial port. It's not terribly efficient, but it works. A video is worth many words:
Hardware:
Controlling the arm was pretty straight forward. I used my servos driver on a PIC18252 running at 20Mhz. The arm is entirely mechanical, ie., it just gives direct control to the servos which move it. I won't bother posting a circuit schematic, it's quite simple. The four servos (one for the pincer, base rotation, upper arm, and lower arm) get plugged into a PCB inside the robot that just routes them to a 6 pin connector (two for power, four for the servos). Then a jumper connects the robot to pins B0, B1, B2, and B3 of the PIC. Of course, the PIC needs power and an oscillator. It's connected to the computer through the internal UART and a max232 chip. Here is a compiled version of the code, which is also displayed right below. And see this page for more information about it.
All of the robot control is handled in robot.h. But the heart of it is the following:
void serial() {
fd = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY);
if (fd < 0) {perror("/dev/ttyS2"); }
struct termios options;
// Get the current options for the port...
tcgetattr(fd, &options);
// Set the baud rates to 19200...
cfsetispeed(&options, B115200);
cfsetospeed(&options, B115200);
// Enable the receiver and set local mode...
options.c_cflag |= (CLOCAL | CREAD);
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8;
// Set the new options for the port...
tcsetattr(fd, TCSANOW, &options);
}
void move() {
if (changeflag) {
char data[4], r;
data[PIVOT] = base_angle_mov;
data[PINCER] = pincer_pos;
data[UPPER] = lower_mov;
data[LOWER] = upper_mov;
write(fd, &data, 4);
usleep(1000);
read(fd, &r, 1);
printf("%c\n", (int)r);
changeflag = 0;
}
}
Software:
I wrote it in C, and I compile it with gcc in ubuntu. Working with the opencv library tends, for me, to be a bunch of trial and error. It would probably be less so if I took the time to learn the math behind some of the algorithms like the canny edge finder, but I'll cross that bridge when I need to. Anyways, here's what the code does to each incoming frame:
Flip the image about the vertical axis so that it's not so confusing moving your hand around. (Otherwise it's like trying to cut your own hair in the mirror)
Convert the image to HSV and get the hue and saturation planes.
Blur them to reduce noise.
Do some binary threshholds on both to select only the colours and saturation levels appropriate to the thimbles. Do an AND (equivalently, in this case, a MAX) over the two resulting binary images to get a single binary image which is white in regions that might be a thimble, and black otherwise.
A great (but ugly) way to get rid of noise, I found, was to then do a sequence of smooths and threshholds on the ANDed image. I found that this gets rid of unconcentrated portions of the image, while leaving the densely blue regions intact.
Do a canny edge finder on our work-in-progress image.
Detect the contours of the resulting image, and loop through them. Approximate each as a polygon. Find it's area, and filter based on this. Take the average position of the vertices, ie. get the center point of the polygon, and store it in a list.
Do a sorting on the output list. The sorting algorithm (in helper.h) looks at the distance between every possible pair of points in the list. It returns a new list of points, whose first two points are the ones that were the closest together, the third and fourth points were the second closest together, etc. Look at the function cvSeqSortByDist for details.
Now just use the information about the closest two points together to control the robot. The control works as follows:
The distance between the points (ie., your two fingers with thimbles) controls the robot's pincers. Close your fingers and the robot squeezes.
The angle between the points (where one finger above the other is 0 degrees) controls the base rotation servo. While the angle is greater than some right threshhold, the robot rotates right, and similarly rotates left if the angle is past some left threshhold. There are actually two threshhold levels on each side. Past the second threshhold, say on the right, the robot will rotate even faster to the right.
The position of the average of the two points controls the upper and lower arm servos. The main screen, with the live webcam feed, is divided into octants: N, NE, E, SE, S, SW, W, NW. N and S control the upper arm. So moving the average position of your fingers into the N region, for example, will cause the robot's upper arm to elevate. The E and W octants control the lower arm. The octants NE, SE, SW, and NW perform combinations of movements. For example, NE will do both the N and E action at the same time, that is, elevate the upper arm while deelevating the lower arm. A central circle is reserve for no movement. Movement starts once the position passes the inner circle. Speed increases once passed the outer circle.