| [sys,x0,str,ts]=navigate(t,x,u,flag,velocitya,loada,velocityb,loadb,fwda,reva,fwdb,revb)
|
% NAVIGATE generates the animation of Differentially Driven Vehicle Robot.
% =======================
% Author: Jahanzeb Rajput
% =======================
% Copyright (c) 2013 by Jahanzeb Rajput
% University of Engineering and Technology, Lahore, Pakistan.
%
% Version: 2.0; Date: January 20, 2013
% Developed at Department of Electronics, Telecommunication and Bio-medical Engineering
% Mehran University of Engineering and Technology Jamshoro, Pakistan.
function [sys,x0,str,ts]=navigate(t,x,u,flag,velocitya,loada,velocityb,loadb,fwda,reva,fwdb,revb)
switch flag,
case 0,
[sys,x0,str,ts]=mdlInitializeSizes(velocitya,loada,velocityb,loadb,fwda,reva,fwdb,revb);
case 2,
sys=mdlUpdate(t,x,u);
case {1,3,4,9},
sys=[];
case 'DeleteBlock',
LocalDeleteBlock;
case 'DeleteFigure',
LocalDeleteFigure;
case 'Slider1',
LocalSlider1;
case 'Slider2',
LocalSlider2;
case 'Slider3',
LocalSlider3;
case 'Slider4',
LocalSlider4;
case 'Slider5',
LocalSlider5;
case 'Slider6',
LocalSlider6;
case 'Slider7',
LocalSlider7;
case 'Slider8',
LocalSlider8;
case 'Close',
LocalClose;
otherwise
error(['Unhandled flag=',num2str(flag)]);
end
%=====================================================================
function [sys,x0,str,ts]=mdlInitializeSizes(velocitya,loada,velocityb,loadb,fwda,reva,fwdb,revb)
sizes=simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates = 0;
sizes.NumOutputs = 0;
sizes.NumInputs = 10;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;
sys=simsizes(sizes);
x0=[];
str=[];
ts=[0.1 0];
LocalPositionInit(velocitya,loada,velocityb,loadb,fwda,reva,fwdb,revb);
%---------------------------------------------------------------------
function sys=mdlUpdate(t,x,u)
fig=get_param(gcbh,'UserData');
if ishandle(fig),
if strcmp(get(fig,'Visible'),'on'),
ud=get(fig,'UserData');
LocalPositionSets(t,ud,u);
end
end;
sys=[];
%---------------------------------------------------------------------
function LocalDeleteBlock
fig=get_param(gcbh,'UserData');
if ishandle(fig),
delete(fig);
set_param(gcbh,'UserData',-1)
end
%---------------------------------------------------------------------
function LocalDeleteFigure
ud=get(gcbf,'UserData');
set_param(ud.Block,'UserData',-1);
%---------------------------------------------------------------------
function LocalSlider1
ud=get(gcbf,'UserData');
set_param(ud.velocitya,'Value',num2str(get(gcbo,'Value')));
%---------------------------------------------------------------------
function LocalSlider2
ud=get(gcbf,'UserData');
set_param(ud.loada,'Value',num2str(get(gcbo,'Value')));
%---------------------------------------------------------------------
function LocalSlider3
ud=get(gcbf,'UserData');
set_param(ud.velocityb,'Value',num2str(get(gcbo,'Value')));
%---------------------------------------------------------------------
function LocalSlider4
ud=get(gcbf,'UserData');
set_param(ud.loadb,'Value',num2str(get(gcbo,'Value')));
%---------------------------------------------------------------------
function LocalSlider5
ud=get(gcbf,'UserData');
set_param(ud.fwda,'Value',num2str(get(gcbo,'Value')));
%---------------------------------------------------------------------
function LocalSlider6
ud=get(gcbf,'UserData');
set_param(ud.reva,'Value',num2str(get(gcbo,'Value')));
%---------------------------------------------------------------------
function LocalSlider7
ud=get(gcbf,'UserData');
set_param(ud.fwdb,'Value',num2str(get(gcbo,'Value')));
%---------------------------------------------------------------------
function LocalSlider8
ud=get(gcbf,'UserData');
set_param(ud.revb,'Value',num2str(get(gcbo,'Value')));
%---------------------------------------------------------------------
function LocalClose
delete(gcbf);
%---------------------------------------------------------------------
function LocalPositionSets(time,ud,u)
mu12=u(3)+u(8);
mu22=u(4)+u(8);
mu32=u(5)+u(8);
mu42=u(6)+u(8);
x1=u(1)*cos(mu12)+u(9);
x2=u(1)*cos(mu22)+u(9);
x3=u(2)*cos(mu32)+u(9);
x4=u(2)*cos(mu42)+u(9);
y1=u(1)*sin(mu12)+u(10);
y2=u(1)*sin(mu22)+u(10);
y3=u(2)*sin(mu32)+u(10);
y4=u(2)*sin(mu42)+u(10);
set(ud.vehicle,...
'XData',[x3 x4;x2 x1],...
'YData',[y3 y4;y2 y1]);
pause(0)
drawnow
%--------------------------------------------------
function LocalPositionInit(velocitya,loada,velocityb,loadb,fwda,reva,fwdb,revb)
sys=get_param(gcs,'Parent');
TimeClock=0;
velocityasignal=str2num(get_param([sys '/' velocitya],'Value'));
loadasignal=str2num(get_param([sys '/' loada],'Value'));
velocitybsignal=str2num(get_param([sys '/' velocityb],'Value'));
loadbsignal=str2num(get_param([sys '/' loadb],'Value'));
fwdasignal=str2num(get_param([sys '/' fwda],'Value'));
revasignal=str2num(get_param([sys '/' reva],'Value'));
fwdbsignal=str2num(get_param([sys '/' fwdb],'Value'));
revbsignal=str2num(get_param([sys '/' revb],'Value'));
angle=0;
XCentre=0;
YCentre=0;
radius1=0;
radius2=0;
inc1=0;
inc2=0;
inc3=0;
inc4=0;
mu12=inc1+angle;
mu22=inc2+angle;
mu32=inc3+angle;
mu42=inc4+angle;
x1=radius1*cos(mu12)+XCentre;
x2=radius1*cos(mu22)+XCentre;
x3=radius2*cos(mu32)+XCentre;
x4=radius2*cos(mu42)+XCentre;
y1=radius1*sin(mu12)+YCentre;
y2=radius1*sin(mu22)+YCentre;
y3=radius2*sin(mu32)+YCentre;
y4=radius2*sin(mu42)+YCentre;
Fig=get_param(gcbh,'UserData');
if ishandle(Fig),
FigUD=get(Fig,'UserData');
set(FigUD.vehicle,...
'XData',[x3 x4;x2 x1],...
'YData',[y3 y4;y2 y1]);
figure(Fig);
return
end
FigureName='Differential Drive Vehicle Robot Navigation (Created By Jahanzeb Rajput)';
Fig=figure(...
'Units', 'pixel',...
'Position', [6 31 791 495],...
'Name', FigureName,...
'NumberTitle', 'off',...
'IntegerHandle', 'off',...
'HandleVisibility', 'callback',...
'Resize', 'on',...
'DeleteFcn', 'navigate([],[],[],''DeleteFigure'')',...
'CloseRequestFcn', 'navigate([],[],[],''Close'');');
AxesH=axes(...
'Parent', Fig,...
'Units', 'pixel',...
'Position', [30 60 750 465],...
'Clim', [1 64],...
'Xlim', [-3 3],...
'Ylim', [-0.5 4],...
'Visible', 'on',...
'XGrid', 'on',...
'YGrid', 'on');
vehicle=surface(...
'Parent', AxesH,...
'XData', [x3 x4;x2 x1],...
'YData', [y3 y4;y2 y1],...
'ZData', zeros(2),...
'CData', ones(2),...
'EraseMode', 'xor');
SlideControl1=uicontrol(...
'Parent', Fig,...
'Style', 'slider',...
'Units', 'pixel',...
'Position', [100 24 100 15],...
'Min', 0,...
'Max', 15,...
'Value', velocityasignal,...
'Callback', 'navigate([],[],[],''Slider1'');');
SlideControl2=uicontrol(...
'Parent', Fig,...
'Style', 'slider',...
'Units', 'pixel',...
'Position', [275 24 100 15],...
'Min', 0,...
'Max', 1.5,...
'Value', loadasignal,...
'Callback', 'navigate([],[],[],''Slider2'');');
SlideControl3=uicontrol(...
'Parent', Fig,...
'Style', 'slider',...
'Units', 'pixel',...
'Position', [100 6 100 15],...
'Min', 0,...
'Max', 15,...
'Value', velocitybsignal,...
'Callback', 'navigate([],[],[],''Slider3'');');
SlideControl4=uicontrol(...
'Parent', Fig,...
'Style', 'slider',...
'Units', 'pixel',...
'Position', [275 6 100 15],...
'Min', 0,...
'Max', 1.5,...
'Value', loadbsignal,...
'Callback', 'navigate([],[],[],''Slider4'');');
SlideControl5=uicontrol(...
'Parent', Fig,...
'Style', 'slider',...
'Units', 'pixel',...
'Position', [430 12 45 15],...
'Min', 0,...
'Max', 1,...
'Value', fwdasignal,...
'Callback', 'navigate([],[],[],''Slider5'');');
SlideControl6=uicontrol(...
'Parent', Fig,...
'Style', 'slider',...
'Units', 'pixel',...
'Position', [480 12 45 15],...
'Min', 0,...
'Max', 1,...
'Value', revasignal,...
'Callback', 'navigate([],[],[],''Slider6'');');
SlideControl7=uicontrol(...
'Parent', Fig,...
'Style', 'slider',...
'Units', 'pixel',...
'Position', [550 12 45 15],...
'Min', 0,...
'Max', 1,...
'Value', fwdbsignal,...
'Callback', 'navigate([],[],[],''Slider7'');');
SlideControl8=uicontrol(...
'Parent', Fig,...
'Style', 'slider',...
'Units', 'pixel',...
'Position', [600 12 45 15],...
'Min', 0,...
'Max', 1,...
'Value', revbsignal,...
'Callback', 'navigate([],[],[],''Slider8'');');
uicontrol(...
'Parent', Fig,...
'Style', 'pushbutton',...
'Position', [700 6 75 30],...
'String', 'Close',...
'Callback', 'navigate([],[],[],''Close'');');
uicontrol(...
'Parent', Fig,...
'Style', 'text',...
'Units', 'pixel',...
'Position', [30 23 60 16], ...
'HorizontalAlignment','right',...
'String', 'V1(rad/sec):');
uicontrol(...
'Parent', Fig,...
'Style', 'text',...
'Units', 'pixel',...
'Position', [30 6 60 16], ...
'HorizontalAlignment','right',...
'String', 'V2(rad/sec):');
uicontrol(...
'Parent', Fig,...
'Style', 'text',...
'Units', 'pixel',...
'Position', [230 23 35 16], ...
'HorizontalAlignment','right',...
'String', 'Load1:');
uicontrol(...
'Parent', Fig,...
'Style', 'text',...
'Units', 'pixel',...
'Position', [230 6 35 16], ...
'HorizontalAlignment','right',...
'String', 'Load2:');
uicontrol(...
'Parent', Fig,...
'Style', 'text',...
'Units', 'pixel',...
'Position', [436 26 205 16], ...
'HorizontalAlignment','left',...
'String', '0 F1 1 0 R1 1 0 F2 1 0 R2 1');
FigUD.vehicle = vehicle;
FigUD.SlideControl1 = SlideControl1;
FigUD.SlideControl2 = SlideControl2;
FigUD.SlideControl3 = SlideControl3;
FigUD.SlideControl4 = SlideControl4;
FigUD.SlideControl5 = SlideControl5;
FigUD.SlideControl6 = SlideControl6;
FigUD.SlideControl7 = SlideControl7;
FigUD.SlideControl8 = SlideControl8;
FigUD.Block = get_param(gcbh,'handle');
FigUD.velocitya = get_param([sys '/' velocitya],'Handle');
FigUD.loada = get_param([sys '/' loada],'Handle');
FigUD.velocityb = get_param([sys '/' velocityb],'Handle');
FigUD.loadb = get_param([sys '/' loadb],'Handle');
FigUD.fwda = get_param([sys '/' fwda],'Handle');
FigUD.reva = get_param([sys '/' reva],'Handle');
FigUD.fwdb = get_param([sys '/' fwdb],'Handle');
FigUD.revb = get_param([sys '/' revb],'Handle');
set(Fig,'UserData',FigUD);
drawnow
set_param(gcbh,'UserData',Fig);
|
|
