Arduino Processing communication repeating action - image-processing

Question

I have made a robotic arm. I need it to capture video and detect blue and red color. And move the blue object to left and red object to right. But the problem is, whenever it is finding a color, it is supposed to send a character '1' (for blue) and '2' for red to the serial port. My problem is,
1. First of all it is taking a lot of time to react.
2. If it detects red. sends '2' to the serial port. But it repeats the action. Even if I change object.
3. Or If it detects red. sends '1' to the serial port. But it repeats the action. Even if I change object.
Color is being detected successfully.
Please someone help me. I've got stuck for a long time.
My Processing Code:
import processing.video.*;
import processing.serial.*;
Serial myPort; // Create object from Serial class
String portName;
Capture video;
color trackColorgreen,trackColorblue,trackColoryellow;
float threshold = 20;
float distThreshold = 75;
ArrayList<Blob> blobgreen = new ArrayList<Blob>();
ArrayList<Blob> blobblue = new ArrayList<Blob>();
ArrayList<Blob> blobyellow = new ArrayList<Blob>();
void findcolorgreen()
{
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(trackColorgreen);
float g2 = green(trackColorgreen);
float b2 = blue(trackColorgreen);
float d = distSq(r1, g1, b1, r2, g2, b2);
if (d < threshold*threshold) {
boolean found = false;
for (Blob b : blobgreen) {
if (b.isNear(x, y)) {
b.add(x, y);
found = true;
break;
}
}
if (!found) {
Blob b = new Blob(x, y);
blobgreen.add(b);
}
}
}
}
}
void findcolorblue()
{
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(trackColorblue);
float g2 = green(trackColorblue);
float b2 = blue(trackColorblue);
float d = distSq(r1, g1, b1, r2, g2, b2);
if (d < threshold*threshold) {
boolean found = false;
for (Blob b : blobblue) {
if (b.isNear(x, y)) {
b.add(x, y);
found = true;
break;
}
}
if (!found) {
Blob b = new Blob(x, y);
blobblue.add(b);
}
}
}
}
}
void setup() {
size(640, 360);
String[] cameras = Capture.list();
printArray(cameras);
video = new Capture(this, cameras[3]);
video.start();
//trackColorgreen = color(11,77,28);
trackColorgreen = color(159,211,207);
//trackColorblue = color( 22,135,217);
trackColorblue = color(195,173,175);
//trackColoryellow = color(179,136,9);
// trackColoryellow = color(221,228,125);
portName = Serial.list()[0]; //change the 0 to a 1 or 2 etc. to match your port
myPort = new Serial(this, portName, 9600);
}
void captureEvent(Capture video) {
video.read();
}
void draw() {
video.loadPixels();
image(video, 0, 0);
blobgreen.clear();
blobyellow.clear();
blobblue.clear();
//threshold = map(mouseX, 0, width, 0, 100);
threshold = 80;
// Begin loop to walk through every pixel
findcolorgreen();
findcolorblue();
if(blobgreen.size()==0 && blobblue.size()==0 && blobyellow.size()==0)myPort.write('0');
for (Blob b : blobblue) {
if (b.size() > 800) {
b.show();
myPort.write('1');
delay(2000);
println("blue");
}
}
for (Blob b : blobgreen) {
if (b.size() > 800) {
b.show();
myPort.write('2');
delay(2000);
println("green");
}
}
}
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;
color trackColor = video.pixels[loc];
}
class Blob {
float minx;
float miny;
float maxx;
float maxy;
Blob(float x, float y) {
minx = x;
miny = y;
maxx = x;
maxy = y;
}
void show() {
stroke(0);
fill(255);
strokeWeight(2);
rectMode(CORNERS);
rect(minx, miny, maxx, maxy);
}
void add(float x, float y) {
minx = min(minx, x);
miny = min(miny, y);
maxx = max(maxx, x);
maxy = max(maxy, y);
}
float size() {
return (maxx-minx)*(maxy-miny);
}
boolean isNear(float x, float y) {
float cx = (minx + maxx) / 2;
float cy = (miny + maxy) / 2;
float d = distSq(cx, cy, x, y);
if (d < distThreshold*distThreshold) {
return true;
} else {
return false;
}
}
}
Arduino Code:
#include <Servo.h>
Servo s2,s3,s4,s5,s6;
int pos;
char val;
void setup() {
s2.attach(3);
s3.attach(4);
s4.attach(5);
s5.attach(6);
s6.attach(7);
s2.write(90);
s3.write(10);
s4.write(100);
s5.write(70);
s6.write(15);
}
void grab()
{
for(pos=15;pos<=85;pos+=1)
{
s6.write(pos);
s5.write(85-pos);
delay(20);
}
delay(50);
// moving s3 forward
for (pos = 10; pos <= 60; pos += 1) {
s3.write(pos);
delay(20);
}
delay(50);
// moving s4 forward
for (pos = 100; pos >= 80; pos -= 1) {
s4.write(pos);
delay(20);
}
delay(20);
// moving s3 forward
for (pos = 60; pos <= 70; pos += 1) {
s3.write(pos);
delay(20);
}
for(pos=85;pos>=15;pos-=1)
{
s6.write(pos);
int x=-(pos-85);
s5.write(x);
delay(20);
}
s5.write(80);
delay(20);
//s3 initial
// moving s3 forward
for (pos = 70; pos >= 10; pos -= 1) {
s3.write(pos);
delay(20);
}
delay(20);
// moving s4 forward
for (pos = 80; pos <= 100; pos += 1) {
s4.write(pos);
delay(20);
}
delay(20);
}
void move_left()
{
for (pos = 90; pos <= 180; pos += 1) {
s2.write(pos);
delay(20);
}
delay(20);
// moving s3 forward
for (pos = 10; pos <= 60; pos += 1) {
s3.write(pos);
delay(20);
}
delay(20);
// moving s4 forward
for (pos = 100; pos >= 70; pos -= 1) {
s4.write(pos);
delay(20);
}
delay(50);
// moving s3 forward
for (pos = 60; pos <= 70; pos += 1) {
s3.write(pos);
delay(20);
}
//expanding claw
for(pos=15;pos<=85;pos+=1)
{
s6.write(pos);
s5.write(85-pos);
delay(20);
}
delay(20);
// moving s4 up
for (pos = 70; pos <=110; pos += 1) {
s4.write(pos);
delay(20);
}
delay(20);
//s3 initial
// moving s3 forward
for (pos = 70; pos >= 10; pos -= 1) {
s3.write(pos);
delay(20);
}
delay(20);
// moving s4 forward
for (pos = 110; pos >= 100; pos -= 1) {
s4.write(pos);
delay(20);
}
delay(20);
//For servo 1 rotation
for (pos = 180; pos >= 90; pos -= 1) {
s2.write(pos);
delay(20);
}
delay(20);
for(pos=85;pos>=15;pos-=1)
{
s6.write(pos);
int x=-(pos-85);
s5.write(x);
delay(20);
}
s5.write(80);
delay(20);
}
void move_right()
{
for (pos = 90; pos >= 0; pos -= 1) {
s2.write(pos);
delay(50);
}
delay(50);
delay(50);
// moving s3 forward
for (pos = 10; pos <= 60; pos += 1) {
s3.write(pos);
delay(50);
}
delay(50);
// moving s4 forward
for (pos = 100; pos >= 70; pos -= 1) {
s4.write(pos);
delay(50);
}
delay(50);
// moving s3 forward
for (pos = 60; pos <= 70; pos += 1) {
s3.write(pos);
delay(50);
}
//expanding claw
for(pos=15;pos<=85;pos+=1)
{
s6.write(pos);
s5.write(85-pos);
delay(20);
}
delay(50);
// moving s4 up
for (pos = 70; pos <=110; pos += 1) {
s4.write(pos);
delay(50);
}
delay(50);
// s3.write(10);
//delay(500);
//s4.write(100);
//s3 initial
// moving s3 forward
for (pos = 70; pos >= 10; pos -= 1) {
s3.write(pos);
delay(50);
}
delay(50);
// moving s4 forward
for (pos = 110; pos >= 100; pos -= 1) {
s4.write(pos);
delay(50);
}
delay(50);
//For servo 1 rotation
for (pos = 0; pos <= 90; pos += 1) {
s2.write(pos);
delay(50);
}
delay(50);
for(pos=85;pos>=15;pos-=1)
{
s6.write(pos);
int x=-(pos-85);
s5.write(x);
delay(20);
}
s5.write(80);
delay(50);
}
void loop() {
if (Serial.available())
{
val = Serial.read(); // read it and store it in val
if(val=='1')
{
grab();
delay(200);
move_left();
delay(200);
}
else if(val=='2')
{
grab();
delay(200);
move_right();
delay(200);
}
}
}
My robot:
https://drive.google.com/open?id=0B9JjLX3LHgUSSU40U3YzOU1LYXc