Пакет MatLab. Элементы программирования. Функции пакета. Графические возможности пакета, страница 17

M=[M zeros(2,k-2); zeros(k-2,2) eye(k-2,k-2)];

function [y1,y2]=azvplot(y1,y2,y1s,y2s)

%Вывод движения k-звенного робота

% использование:  [y1s,y2s]=azvplot(y1,y2,y1s,y2s)

%вход: y1, y2  - массивы координат для рисования

%           y1s,y2s - массивы координат для стирания

%выход: y1, y2  - массивы координат для стирания

plot(y1s,y2s,'i',y1s,y2s,'*i'); plot(y1,y2,'c13',y1,y2,'*c6');

function [k,nk]=azpodg(l,q,nk,yz)

%Проверка размерностей массивов,

%подготовка экрана для вывода k-звенного робота

% l, q - массивы длин звеньев и углов

% nk - массив номеров звеньев для вывода звеньев

% yz - уставки для избыточных звеньев

if length(l)~=length(q) error('Размерности l и q различны'); end

for i=1:length(nk), if nk(i)>length(l) nk(i)=length(l); end; end

axis('square'); k=length(l);

m=ceil(sum(l)); v=[-m m -m m]; axis(v); plot(-m,-m,m,m); hold on;

x=[0 -m/30 m/30 0]; y=[0 -m/15 -m/15 0]; plot(x,y,'c6');

for i=1:k-2, st=sprintf('yz%g = %g',k-i,yz(i));

text(0.25,0.9-0.03*i,st,'sc'); end;

st=sprintf('Избыточный %g-звенный манипулятор',k); title(st);

function [y,et]=azvryetl(l,q,al,s0,p0,yz,ry)

%Расчет векторов y и et

% l - массив длин, q - массив углов,

% al, s0, p0 - параметры линии, yz - вектор задания

k=length(l); qs=0; y=zeros(k,1);

for i=1:k, qs=qs+q(i); y(1)=y(1)+l(i)*cos(qs); y(2)=y(2)+l(i)*sin(qs); end

for j=1:k-2, qs=-al;

for i=1:k-j, qs=qs+q(i); y(j+2)=y(j+2)+l(i)*sin(qs); end; end;

y(3:k)=y(3:k)+s0;

% Случай 2 для 3 звенного

%y1p=l(1)*cos(q(1))+l(2)*cos(q(1)+q(2));

%y2p=l(1)*sin(q(1))+l(2)*sin(q(1)+q(2));

%y(3:k)=cos(al)*(y(1)-y1p)+sin(al)*(y(2)-y2p);

et=[y(1)*cos(al)+y(2)*sin(al)+p0; y(2)*cos(al)-y(1)*sin(al)+s0; y(3:k)-yz'];

function [x,y]=azvrysh(l,q)

%Расчет положения k-звенного робота

%l - массив длин, q - массив углов, x, y - координаты концов звеньев

k=length(l); qs=0; x=0; y=0;

for i=1:k, qs=qs+q(i);

x(i+1)=x(i)+l(i)*cos(qs); y(i+1)=y(i)+l(i)*sin(qs); end

function [sx,sy]=azvsp(l,nk,x,y,sx,sy)

%Вывод следа движения k-го звена

% l - массив длин звеньев, nk - массив номеров звеньев для вывода,

% x, y - массивы координат звеньев, sx, sy - массив для запоминания следа,

for i=1:length(nk), ssx(i)=x(nk(i)+1); ssy(i)=y(nk(i)+1); end

sx=[sx; ssx]; sy=[sy; ssy];

for i=1:length(nk), plot(sx(:,i),sy(:,i),'c9'); end

function [u,et]=azvu(l,q,al,vz,K,et,M,u,ry)

%Расчет управления u=inv(M*J)*K*et

k=length(l); J=zeros(k+1,k+1);  for i=k:-1:1, qs=0;

for j=1:i, qs=qs+q(j); end

J(1,i)=J(1,i+1)-l(i)*sin(qs); J(2,i)=J(2,i+1)+l(i)*cos(qs); end

for ii=3:k, for i=k-ii+2:-1:1, qs=-al;

for j=1:i, qs=qs+q(j); end

J(ii,i)=J(ii,i+1)+l(i)*cos(qs); end; end;

% Случай 2 для 3 звенного

%J(3,1)=-l(3)*sin(q(1)+q(2)+q(3)-al);

%J(3,2)=-l(3)*sin(q(1)+q(2)+q(3)-al);

%J(3,3)=-l(3)*sin(q(1)+q(2)+q(3)-al);

J=J(1:k,1:k);

P=inv(M*J); v=J(1,:)*u*cos(al)+J(2,:)*u*sin(al);

et(1)=v-vz; u=K*et; u(1)=u(1)+vz; u=P*u;

§3. Пользовательский интерфейс (исследование влияния коэффициентов характеристического уравнения на качество переходных процессов).

Требуемые функции: abcdchk.m, c2d.m, lsim.m, series.m, tf2ss.m.

function lab2(action,in1,in2);

if nargin<1,  action='start';  end;

global LAB2_D

if strcmp(action,'start') clf reset;  

screen=get(0,'ScreenSize');

set(gcf,'Units','pixels','backingstore','off',...

'Position',[0 0 screen(3) screen(4)],...

'Name','Laboratorium  N 2','Resize','off','NumberTitle','off');

it0=.01; jt0=.04; its=.12; jts=.03;

is0=it0+.03; js0=jt0+.06;  iss=.02; jss=.3;

a1=2;  a2=4;  b=0;  x0=[1 0]'; dt=0.01; Tk=10;

min_a1=-1; max_a1=19;  min_a2=-1; max_a2=19;

min_x1=-1; max_x1=1;  min_x2=-1; max_x2=1;

A=[0 1; -a2 -a1]; B=[0; b]; C=[1 0]; D=0; t=0:dt:Tk; u=ones(1,Tk/dt+1);

[y,x]=lsim(A,B,C,D,u,t,x0);

p=eig(A); ip=imag(p); rp=real(p);

mi=max(ip); if(abs(min(ip))>mi) mi=abs(min(ip)); end

mr=max(rp); if(abs(min(rp))>mr) mr=abs(min(rp)); end

mm=mr; if(mi>mr) mm=mi; end

mm=abs(ceil(1.1*mm));

x1m=abs(min(x(:,1))); if(max(x(:,1))>x1m) x1m=max(x(:,1)); end

x2m=abs(min(x(:,2))); if(max(x(:,2))>x2m) x2m=max(x(:,2)); end

xm=x1m; if(x2m>xm) xm=x2m; end

xm=abs(ceil(1.1*xm));

axis([0 600 0 480]); axis('off'); lx=160; ly=150;