Virtual Eye System -- Post #8 -- Code optimisation and Error correction

8 Nov 2015

Hi guys!!!

Although the Teacher's Pet Students' Robotics Challenge has concluded, I found that the code mentioned in my previous blog post has some bugs and I have optimised it in this post and have added a video showing the same. The response is faster and better than the previous one.

MATLAB code

clear rpi
clear cam
rpi=raspi();
myserialdevice = serialdev(rpi,'/dev/ttyAMA0',9600);
cam = cameraboard(rpi, 'Resolution', '640x480');
cam.Rotation=180;
edit('track.m');thresh=50;
figure(1);
for i = 1:500
    %% Extract RGB color components
    img=snapshot(cam);
r = img(:,:,1);
g = img(:,:,2);
b = img(:,:,3);

%% Calculate green
justGreen =g - b/2 - r/2;
%% Threshold the image
bw = justGreen > thresh;


%% Find center
[x, y] = find(bw);
if ~isempty(x) && ~isempty(y)
    xm = round(mean(x));
    ym = round(mean(y));
    fprintf('x-coordinate: %d\n', xm);    
    fprintf('y-coordinate: %d\n', ym);
    if xm<200 
        if ym<200
            write(myserialdevice,[1]);%inside ''...it is normal values w/o.....it takes ASCII
        elseif ym>500
            write(myserialdevice,[2]);
        end
    elseif xm>300
        if ym<200
            write(myserialdevice,[3]);%inside ''...it is normal values w/o.....it takes ASCII
        elseif ym>500
            write(myserialdevice,[4]);
        end
   end
    %% Creat a red dot
    xx = max(1, xm-5):min(xm+5, size(bw, 1));
    yy = max(1, ym-5):min(ym+5, size(bw, 2));
    bwbw = zeros(size(bw), 'uint8');
    bwbw(xx, yy) = 255;
    
    %% Create output image
    img(:,:,1) = uint8(r + bwbw);
    img(:,:,2) = uint8(g - bwbw);
    img(:,:,3) = uint8(b - bwbw);
end
    subplot(211);
    imagesc(img);
    subplot(212);
    imagesc(bw);
    drawnow;


end

Instead of sending signed integer, we are now sending unsigned integers for four possible cases of an object being in either of the four corners of the camera frame.

The corresponding arduino code for controlling the servo is as follows:

#include <Servo.h> 
//Orange Signal
//Red Positive
//Brown GND
Servo myservo1;  
Servo myservo2;

int x,y,i=0,j=0;   

void setup() 
{ 
  myservo1.attach(A0);  
  myservo2.attach(A1);
  myservo1.write(45);
  myservo2.write(120);
  Serial.begin(9600);
  } 

void loop() 
{   while(Serial.available()>0){
  x=Serial.read();
  
  i=myservo1.read();
  j=myservo2.read();
  Serial.println(x);
  if(i<=90 && i>=0 && j<=180 && j>=90){
  if(x==1){
    i=myservo1.read();
    myservo1.write(i+2);
    j=myservo2.read();
    myservo2.write(j+2);}
  else if(x==2){
    i=myservo1.read();
    myservo1.write(i-2);
    j=myservo2.read();
    myservo2.write(j+2);}
  else if(x==3){
    i=myservo1.read();
    myservo1.write(i+2);
    j=myservo2.read();
    myservo2.write(j-2);}
  else if(x==4){
  i=myservo1.read();
    myservo1.write(i-2);
    j=myservo2.read();
    myservo2.write(j-2);}
}
else {myservo1.write(45);
myservo2.write(120);}
}
}

As said, the four cases are handled accordingly by turning the servo by 2 degrees per reading received.

Happy Coding!!!