% 2a. dan racunskih vezbi iz Tehnicke robotike u zimskom semestru skolske 1998/99
% 26. 11. 1998.

%*************
% Zadatak 1. *
%*************

clear all

%***********************************************
disp('Matrica DH parametara Mitsubishi robota:')

%     alfa   a   teta  d    sigma
dh = [pi/2   0    0    214   0
       0    200   0    0     0
       0    150   0    0     0
      pi/2   0    0    0     0 
       0     0    0    85    0]

disp('Pritisni razmaknicu da bi nastavio rad!');
pause;

%*****************************
disp('Ugaoni opsezi zglobova')

tmin = [-120 -30 -120   0 -180]
tmax = [ 120 120    0 180  180]

%*********************************************************************************
disp('Generisanje slucajnog vektora unutrasnjih koordinata q1d [deg] (q1 [rad]):');

q1d = tmin' + rand(5,1).*(tmax - tmin)' % q1 [deg]
q1 = q1d.*pi/180 % q1 [rad]

disp('Pritisni razmaknicu da bi nastavio rad!');
pause;

disp('  ');

%***********************************************************************************************
disp('RACUNANJE DIREKTNE KINEMATIKE NA BAZI SIMBOLICKIH IZRAZA IZ FUNKCIJSKE DATOTEKE mitsufk.m')

flops(0);

T5s = mitsufk(q1)

fprintf('flops = %10.0f\n\n',flops)

disp('Pritisni razmaknicu da bi nastavio rad!');
pause;

disp('  ');

%******************************************************************************
disp('NUMERICKO RACUNANJE DIREKTNE KINEMATIKE POMOCU FUNKCIJSKE DATOTEKE fkine')

flops(0);

T5n = fkine(dh,q1)

fprintf('flops = %10.0f\n\n',flops)

disp('Pritisni razmaknicu da bi nastavio rad!');
pause;

disp('  ');

%***********************************************************************************************
disp('RACUNANJE INVERZNE KINEMATIKE NA BAZI SIMBOLICKIH IZRAZA IZ FUNKCIJSKE DATOTEKE mitsuik.m')

flops(0);

qs = mitsuik(T5s);

fprintf('flops = %10.0f\n',flops)

qsd = qs*180/pi

disp('Pritisni razmaknicu da bi nastavio rad!');
pause;

disp('  ');

%*********************************************************************************
disp('NUMERICKO RACUNANJE INVERZNE KINEMATIKE POMOCU FUNKCIJSKE DATOTEKE ikine.m')

disp('  ');
disp('Zadavanje pocetnog uslova na osnovu q1:')

qguess = q1 + .1*(rand(5,1)-.5);
qguessd = qguess'*180/pi

disp('  ');
disp('Zadavanje tolerancije pri numerickom sracunavanju resenja IK')

tol = 1e-5

disp('  ');
disp('Specificiranje prostornih koordinata u koor. sistemu hvataljke koje se ne mogu kontrolisati nezavisno:')
disp('(ROTACIJA OKO X OSE KOOR. SISTEMA HVATALJKE)')

M = [1 1 1 0 1 1]'

disp('Numerisko resenje IK pomocu ikine.m:')

flops(0)

qn = ikine(dh,T5n,qguess,M,tol)

fprintf('flops = %10.0f\n',flops)

qnd = qn*180/pi

disp('Poredjenje tacnosti numerickog resenja IK:')

disp('  ');
disp('Apsolutna greska: qerror = abs(q1d - qnd) [deg]');
qerror = abs(q1d' - qnd)

disp('Maksimalna greska: maxerror=max(abs(qerror))');
maxerror = max(qerror)

disp('Pritisni razmaknicu da bi nastavio rad!');
pause;

disp('Poredjenje svih resenja:')
disp('  ');

disp('Polazni vektor unutrasnjih koordinata [deg]:')

qzad=q1d'

disp('Resenje IK na bazi simbolickih jednacina [deg]:')

qsd

disp('Pocetni uslov za nalazenje numerickog resenja (deg):')

qguessd

disp('Numericko resenje IK [deg]:')

qnd

