% Script amatrix.m			CREATE SYMBOLIC A MATRIX
% 	Symbolic Robotics Toolbox
% 	Christian Paquette, Chris Harper, Roger Toogood
%	Dept. of Mechanical Engineering
%	University of Alberta

menu2=menu('Creating A Matricies',...
	'Read DH Parameters from file',...
	'Enter DH Parameters',...
	'Mitsubishi Robot DH Parameters',...
	'Revolute RRR Robot DH Parameters',...
	'Spherical RRP Robot DH Parameters',...
	'Cylindrical RPP Robot DH Parameters',...
	'Cartesian PPP Robot DH Parameters',...
	'MAIN MENU');

if ~(menu2==8)						%Checks if MAIN MENU was selected
	if menu2==1					   %Reading DH Parameters from disk file
     disp('Reading DH parameters from a file.')
     [fname, pname] = uigetfile('*.dat','Open DH Parameter file...');

     if fname ~= 0    % does fname exist
        eval(['load ' fname]);
        disp(['DH parameters have been read from file ' fname])
        lfname = length(fname);  % get the root name of file 
        dh = eval(fname(1:lfname-4));  % put parameters into dh array

        [dhrow, dhcol] = size(dh);
        if dhrow > 6
           disp('Warning: May be too many links in DH matrix for efficient operation')
   	  end
	     if dhcol ~= 5
		     disp('ERROR: DH matrix must have five columns')
        end
        %disp(['The number of links is ' dhrow])
        disp('DH parameters are:')
        disp('  alpha    a  theta    d    sigma')
        disp(dh)
        % reset this flag since new parameters have been read
        fkindone = 0;

     else
        disp('...File read operation cancelled.')
     end
   end

	if menu2==2					   %Enter DH Parameters
		disp('Getting DH parameters from user.')
		[dh, dhrow] = getdhpar;
      % reset this flag since new parameters have been read
      fkindone = 0;

	end

	if menu2==3
      disp('Setting DH parameters for Mitsubishi Robot')
		dh= [90 0 0 1 0;
            0 1 0 0 0;
            0 1 0 0 0;
           90 0 0 0 0;
            0 0 0 1 0]
		dhrow=5;
      % reset this flag since new parameters have been read
      fkindone = 0;

	end

	if menu2==4				%RRR Robot
      disp('Setting DH parameters for RRR Robot')
		dh= [90 0 0 1 0;
            0 1 0 0 0;
            0 1 0 0 0]
		dhrow=3;
      % reset this flag since new parameters have been read
      fkindone = 0;
	end

	if menu2==5				%RRP Robot
      disp('Setting DH parameters for RRP Robot')
		dh= [90 0 0 1 0;
          -90 0 0 0 0;
            0 0 0 0 1]
		dhrow=3;
      % reset this flag since new parameters have been read
      fkindone = 0;
	end

	if menu2==6				%RPP Robot
      disp('Setting DH parameters for RPP Robot')
		dh= [ 0 0  0 0 0;
           90 0 90 0 1;
            0 0  0 0 1]
		dhrow=3;
      % reset this flag since new parameters have been read
      fkindone = 0;
	end
	
	if menu2==7				%PPP Robot
      disp('Setting DH parameters for PPP Robot')
		dh= [-90 0  0 0 1;
            90 0 90 0 1;
             0 0  0 0 1]
		dhrow=3;
      % reset this flag since new parameters have been read
      fkindone = 0;

	end

   if dhrow ~= 0
      %call symdh_a to generate the A matrices for each link
      disp('Generating A matrices')
   	for n=1:dhrow
   		eval(['A' num2str(n) '=symdh_a(dh,n)']);
   	end	
   end
end
