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