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);

% calculate velocities

err = posx - round( length( img ) / 2 );

if( posx == 0 || posx == 320 )

vx = 0;

vy = 20;

omega = sign( err )*(-10);

else

if( (300 - abs( err )) > 10 )

vx = 300 - abs( err );

else

vx = 10;

end

vy = floor( - err/2 );

omega = floor( - err/2 );

end;

OmniDrive_setVelocity(OmniDriveId, vx, vy ,omega);

end;

end;

Com_disconnect(ComId);

Camera_destroy(CameraId);

Bumper_destroy(BumperId);

OmniDrive_destroy(OmniDriveId);

Com_destroy(ComId);

...