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

Jahanzeb Rajput (view profile)

 

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