Robotino

ComId = Com_construct;

OmniDriveId = OmniDrive_construct;

CameraId = Camera_construct;

BumperId = Bumper_construct;

Com_setAddress(ComId, '127.0.0.1:8081');

Com_connect(ComId);

OmniDrive_setComId(OmniDriveId, ComId);

Camera_setComId(CameraId, ComId);

Bumper_setComId(BumperId, ComId);

tStart = tic;

while (Bumper_value(BumperId) ~= 1)

tElapsed = toc(tStart);

% If 60 seconds are elapsed then exit while loop

if(tElapsed >= 60 )

break;

end;

if ~(Camera_setStreaming(CameraId, 1) == 1)

disp('Camera_setStreaming failed.');

end;

if (Camera_grab(CameraId) == 1)

img = Camera_getImage( CameraId );

threshold = 50;

image(img); title('An image from the camera onboard Robotino')

% Prewitt-Filter

Fx = [-1 0 1;-1 0 1;-1 0 1];

Fy = [-1 -1 -1;0 0 0;1 1 1];

% region of interest:

[m,n,c] = size( img );

roi = zeros(20,n,3);

roi = img(m-19:m,:,:);

% grayscale:

gray = zeros(20,n);

gray(:,:) = floor( (1/3)*( roi(:,:,1) + roi(:,:,2) + roi(:,:,3) ) );

% edge detection with Prewitt filters:

edge = zeros(20,n);

edge(:,:) = abs( floor( (1/2)*( convn(gray,Fx,'same')+convn(gray,Fy,'same') ) ) );

% threshold

bw = zeros(20,n);

for(i=1:20)

for(j=1:n)

if( edge(i,j) < threshold )

bw(i,j) = 0;

else

bw(i,j) = 255;

end;

end;

end;

% calculate x-position of line:

sum = 0;

for(j=2:n-1)

if( bw(1,j) == 0 )

sum = sum + j;

break;

end;

end;

for(j=n-1:-1:2)

if( bw(1,j) == 0 )

sum = sum + j;

break;

end;

end;

for(j=2:n-1)

if( bw(20,j) == 0 )

sum = sum + j;

break;

end;

end;

for(j=n-1:-1:2)

if( bw(20,j) == 0 )

sum = sum + j;

break;

end;

end;

% detected x-position of line

posx = ceil(sum/4);

...