Computer Vision with Processing and Arduino

Using a robotic arm and an endoscope camera I have programmed the arm to follow objects of differing colours. The robot has two axes of motion, this could easily be expanded. The computer vision code is done in Processing based on A brilliant Coding Train example, the sketch sends commands via serial to an Arduino that controls the robots servos.

How it works?

The camera captures an image, the Processing sketch then looks for pixels of a specified colour, it draws a rectangular blob on the image as shown below where it sees the colour.

DSC00961.JPG

Once the blob has been drawn the computer works out where the centre of the rectangle is in x,y coordinates ( with the middle of the screen being 0,0). Depending on where the blob is the sketch sends a command to the Arduino in the form of an array of letters.

For example, if the blob is on the right of the screen, we want to move the arm right to make the blob in the middle of the cameras field of view. The processing sketch will send a ” move right” command to the Arduino. The Arduino moves the servos until the blob is in the middle of the cameras view then stops when a “stop moving” command is sent from Processing.

On the Arduino side, it receives the array from Processing via serial. It looks for a start character “<” and end character “>” then parses the data in between. So it receives “<ab>” then puts “ab” into an array.  The first field of the array corresponds to the x axis, the second corresponds to y. Each field is only ever “a, b or c”. “a” means move left, “b” means move right, “c” means stop.

Proportional control won’t work in this instance as the robotic arm does not know where it is, i.e has no direct position feedback. If you try and work out how far the blob is from the centre and do a proportional move you will get an erratic and unreliable response. This is why it’s kind of slow! This is as far as I’ll take this, for now, the basic code is available below

PROCESSING SKETCH

// Daniel Shiffman
// http://codingtra.in
// http://patreon.com/codingtrain
// Code for: https://youtu.be/1scFcY-xMrI
// Modified for robotic arm tracking by CM Aug 2017

import processing.video.*;
import processing.serial.*;

Capture video;

Serial arduinoPort;
//String portName = “/dev/ttyACM0”;

color trackColor;
float threshold = 25;
float distThreshold = 50;
float lerpX = 320;
int rx ;
int ry ;
char x_pos, y_pos;

ArrayList<Blob> blobs = new ArrayList<Blob>();

void setup() {
size(640, 480);
String portName = “/dev/ttyACM0”;
arduinoPort = new Serial(this, portName, 9600);
// frameRate(4);
//port = new Serial(this, portName, 4800);
String[] cameras = Capture.list();
printArray(cameras);
video = new Capture(this, 640, 480, “/dev/video1”); //uses endoscope video0 uses laptop webcam
video.start();
trackColor = color(255, 0, 0);
}

void captureEvent(Capture video) {
video.read();
}

void keyPressed() {
if (key == ‘a’) {
distThreshold+=5;
} else if (key == ‘z’) {
distThreshold-=5;
}
if (key == ‘s’) {
threshold+=5;
} else if (key == ‘x’) {
threshold-=5;
}

 

println(distThreshold);
}

void draw() {
video.loadPixels();
image(video, 0, 0);

blobs.clear();

 

// Begin loop to walk through every pixel
for (int x = 0; x < video.width; x++ ) {
for (int y = 0; y < video.height; y++ ) {
int loc = x + y * video.width;
// What is current color
color currentColor = video.pixels[loc];
float r1 = red(currentColor);
float g1 = green(currentColor);
float b1 = blue(currentColor);
float r2 = red(trackColor);
float g2 = green(trackColor);
float b2 = blue(trackColor);

float d = distSq(r1, g1, b1, r2, g2, b2);

if (d < threshold*threshold) {

boolean found = false;
for (Blob b : blobs) {
if (b.isNear(x, y)) {
b.add(x, y);
found = true;
break;
}
}

if (!found) {
Blob b = new Blob(x, y);
blobs.add(b);
}
}
}
}

for (Blob b : blobs) {
if (b.size() > 500) {
b.show();
}
}

textAlign(RIGHT);
fill(0);
text(“distance threshold: ” + distThreshold, width-10, 25);
text(“color threshold: ” + threshold, width-10, 50);
for (Blob b : blobs) {
if (b.x_middle() !=0) { //draw blob
float xoff =(320 – b.x_middle()); // get the offset from centre
float x_map = map(xoff, 320, -320, 60, 120); //map to 60-120 modify if you want more resoloution.
int rx = round(x_map); // make it a round int
text(” x offset: ” + rx, width-10, 75); //print on screen
if (rx < 88) { //if blob is on the left of the screen
x_pos = ‘a’; //make the x variable ‘a’
}
if (rx > 92) { //if blob is on right of screen
x_pos =’b’; //make the x variable ‘b’
}
if ((rx <= 92) && (rx >= 88)) { //if the blob is in the middle (make these closer to 90 if you want more accuracy)
x_pos = ‘c’; //make the x varible ‘c’
}
}
}
for (Blob b : blobs) {
if (b.y_middle() !=0) {
float yoff =(240 – b.y_middle()); // get the offset
float y_map = map(yoff, 240, -240, 80, 100);
int ry = round(y_map); // make it a round int
text( ” y offset: ” + ry, width-10, 100); //print on screen
if (ry < 87) {
y_pos = ‘a’;
}
if (ry > 93) {
y_pos = ‘b’;
}
if ((ry <= 93) && (ry >= 87)) {
y_pos = ‘c’;
}
arduinoPort.write(“<“); //write a start char to arduino
arduinoPort.write(x_pos); //write the x axis control char (a,b or c)
arduinoPort.write(y_pos); //write the y axis control char (a,b, or c)
arduinoPort.write(“>”); //write end char
}
}
}

// Custom distance functions w/ no square root for optimization
float distSq(float x1, float y1, float x2, float y2) {
float d = (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1);
return d;
}

 

float distSq(float x1, float y1, float z1, float x2, float y2, float z2) {
float d = (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1) +(z2-z1)*(z2-z1);
return d;
}

void mousePressed() {
// Save color where the mouse is clicked in trackColor variable
int loc = mouseX + mouseY*video.width;
trackColor = video.pixels[loc];
}

############################################################################

PROCESSING SKETCH 2 – (Put in a new tab called blob)

// Daniel Shiffman
// http://codingtra.in
// http://patreon.com/codingtrain
// Code for: https://youtu.be/1scFcY-xMrI

class Blob {
float minx;
float miny;
float maxx;
float maxy;

ArrayList<PVector> points;

Blob(float x, float y) {
minx = x;
miny = y;
maxx = x;
maxy = y;
points = new ArrayList<PVector>();
points.add(new PVector(x, y));
}

void show() {
stroke(0);
fill(255);
strokeWeight(2);
rectMode(CORNERS);
rect(minx, miny, maxx, maxy);
stroke(2);
fill(0);
strokeWeight(2);
fill(255,255,0);
ellipse(320,240,15,15);
for (PVector v : points) {
//stroke(0, 0, 255);
//point(v.x, v.y);

}
}

 

void add(float x, float y) {
points.add(new PVector(x, y));
minx = min(minx, x);
miny = min(miny, y);
maxx = max(maxx, x);
maxy = max(maxy, y);
}

float size() {
return (maxx-minx)*(maxy-miny);
}
float x_middle() {
return minx + ((maxx-minx)/2);
}

float y_middle() {
return miny + ((maxy-miny)/2);
}

boolean isNear(float x, float y) {

// The Rectangle “clamping” strategy
// float cx = max(min(x, maxx), minx);
// float cy = max(min(y, maxy), miny);
// float d = distSq(cx, cy, x, y);

// Closest point in blob strategy
float d = 10000000;
for (PVector v : points) {
float tempD = distSq(x, y, v.x, v.y);
if (tempD < d) {
d = tempD;
}
}

if (d < distThreshold*distThreshold) {
return true;
} else {
return false;
}
}
}

############################################################################

ARDUINO SKETCH

#include <Servo.h>

const byte numChars = 4;
char receivedChars[numChars]; // an array to store the received data

boolean newData = false;

 

Servo xservo;
Servo yservo;

int x_angle = 1500; //midpoint of servo x
int y_angle = 1500;
int speed = 1;
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
xservo.attach(9);
yservo.attach(10);
xservo.write(x_angle);
yservo.write(y_angle); //set both servos to mid pos

delay(500);
}

void loop() {

recvWithStartEndMarkers();
showNewData();

 

}

void recvWithStartEndMarkers() { //recieves new data from processing
static boolean recvInProgress = false;
static byte ndx = 0;
char startMarker = ‘<‘;
char endMarker = ‘>’;
char rc;

// if (Serial.available() > 0) {
while (Serial.available() > 0 && newData == false) {
rc = Serial.read();

if (recvInProgress == true) {
if (rc != endMarker) {
receivedChars[ndx] = rc;
ndx++;
if (ndx >= numChars) {
ndx = numChars – 1;
}
}
else {
receivedChars[ndx] = ‘\0’; // terminate the string
recvInProgress = false;
ndx = 0;
newData = true;
}
}

else if (rc == startMarker) {
recvInProgress = true;
}
}
}

void showNewData() { //if new data recieved move servos
if (newData == true) {
if (receivedChars[0] == ‘a’) { //if x axis char is a move servo +1 increment
x_angle = x_angle + 1;
xservo.write(x_angle);
delay(speed); //delay to allow move to complete, can be tuned.
}

if (receivedChars[0] == ‘b’) { //if x asis is b move serovo -1 increment
x_angle = x_angle – 1;
xservo.write(x_angle);
delay(speed);
}

if (receivedChars[0] == ‘c’) { //if x axis is c do not move

xservo.write(x_angle);
delay(speed);
}

if (receivedChars[1] == ‘a’) { //if y angle is a move servo +1 increment
y_angle = y_angle + 1;
yservo.write(y_angle);
delay(speed);
}

if (receivedChars[1] == ‘b’) {
y_angle = y_angle – 1;
yservo.write(y_angle);
delay(speed);
}

if (receivedChars[1] == ‘c’) {
yservo.write(y_angle);
delay(speed);
}

newData = false;
}
}

 

 

Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s