%============================== testCalib01 ==============================
%
%  script testCalib
%
%  This script moves the piktil to some pre-programmed arm configurations.
%
%  The manipulator will be given a series of joint angle commands 
%  that will move to arm to specific testable configurations.  Testable
%  means that a visual check will confirm proper calibration.  If the
%  calibration is off, then the manipulator visual check will be off.
%
%============================== testCalib01 ==============================

%
%  Name:		testCalib01
%
%  Author:		Patricio A. Vela,		pvela@gatech.edu
%  Created:		2015/03/03
%  Modified:	2023/09/11
%
%============================== testCalib01 ==============================
function testCalib01        

myparms = load('pikparms.mat');     % Change to be your saved file.
pikh = piktul(myparms);

for ii = 1:2            	% Send a couple commands to get it going.
  pikh.gotoSleep();       	%  If doesn't work, increase the loop count.
  pause(0.15);
end

pause(0.5);
pikh.gotoHome();          	% Send it to the home position.
disp('At home position, press return to start ...');
pause();

							% List of joint configurations, row-wise.
                            % Transpose sets to column-wise.
alpha_traj = transpose([0.75 ,   0,   0,   0,  0.5  ;
						1.00 ,   0,   0,   0,  1.0  ;
                        1.50 , -45,   0,  45,  0.25 ; 
                        1.00 ,  45,  45, -30,  0.5  ;
                        0.50 ,  30, -60,  30,  1.0  ;
                        1.25 ,  30,  45, -45,  0.75 ;
                        0.75 ,   0, -60, -30,  1.5  ;
                        2.00 , -15, -30,  45,  0.25 ]);
                            
theStrings = {'Height and gripper adjust.' , ...
              'Height and gripper open.',...
              'Height, -A2, A4, gripper close.',...
              'A2, A3, -A4.',...
              'Height, A2, -A3, A4, gripper open.',...
              'Height, A2, A3, -A4, gripper nominal.',...
              'Height, A3, A4, gripper wide.',...
              'Height top, -A2, -A3, A4, gripper close.'};

for ii = 1:size(alpha_traj,2)   				% Run the routine.
  pikh.setArm(alpha_traj(:,ii), 3);
  disp(' ');
  disp('Verify manipulator joint configuration, then press return.');
  disp(theStrings{ii});
  disp(transpose(alpha_traj(:,ii)));
  pause();      
end             
disp('Done, shutting down.');

pause(1);
pikh.gotoSleep();         	% Goto sleep,
pikh.shutdown();			%    then shutdown.


end


% Other options for alpha_traj
%
%   1, -30, -30, -30, 0.75 ;
%   Check out the arm. Gripper should be x-axis aligned.
%
%   2, -30, -60, 45,  0.25 ;
%   Checkout the arm.  Arm link should be aligned with x-axis.
%   Gripper should be diagonal.
%
%   0.25, 45, -45, -45, 0.25 ;
%   Arm should align with y-axis.
%   Gripper should be diagonal, other way now.
%
%============================== testCalib01 ==============================