Code covered by the BSD License  

Highlights from
Differential Drive and Global Positioning Blockset v2.0

image thumbnail

Differential Drive and Global Positioning Blockset v2.0

by

 

11 Dec 2006 (Updated )

This blockset is for modeling and visualization of the dynamics of differential drive vehicle robot.

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

Contact us