%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%                                                                        %%
%%%                  Written by Matt Kontz                                 %%
%%%                  Walla Walla College                                   %%
%%%                  Edward F. Cross School of Engineering                 %%
%%%                  February 2001                                         %%
%%%                  Simulation of a planar three link robot.              %%
%%%                                                                        %%
%%%      The purpose of this program is to simulate a three link planar    %%		
%%%      robot located in the robots lab at Walla Walla College.  This     %%
%%%      robot demo simulates kinematics, inverse kinematics and real-     %%
%%%      time graphical inverse kinematics.                                %%
%%%                                                                        %%
%%%      To operate this program:                                          %% 
%%%           1)     Make sure that demobot, forkin, invkin, setplot       %%
%%%                       and option are in the appropriate path.          %%       
%%%           2)     Execute demobot.                                      %%
%%%           3)     Select on option from the push button gui.            %%
%%%           4)     Have fun!                                             %%
%%%                                                                        %%
%%%      All inverse and forward kinematics equations where derived by     %%
%%%      Matt Kontz.  The inverse kinematics equations come from basic     %%
%%%      high school level math such as the law of cosines, bouble angle   %%
%%%      formulas, trig identities and the quadratic eqaution.  Since      %%
%%%      this robot has three degrees of freedom and in only 2D a third    %% 
%%%      constaint was required.  The second and third joint angles        %%
%%%      are always equal.                                                 %%
%%%                                                                        %%
%%%      demobot.m  This is the main program which also all the            %%
%%%                       following programs.                              %%
%%%      forkin.m    Inputs three link angles and calculates the new       %%
%%%                       position matrices(forward kinematics).           %%
%%%      invkin.m    Inputs the position is spherical coordinates, does    %% 
%%%                       inverse kinematics and find link angles.         %%
%%%      setplot.m   Uses the data calculated in forkin to update the      %%   
%%%                       figure window.                                   %%
%%%      option.m    This program executes the various options: position   %% 
%%%                       sliders, click on target, click and drag and     %%
%%%                       angles sliders.                                  %%
%%%                                                                        %%
%%%      If you have question about this program email Don Riley           %%
%%%      <riledo@wwc.edu> or Matt Kontz <mkontz@mail.com>.  Don Riley is   %%
%%%      the Robotics professor at Walla Walla College and is responsible  %%
%%%      for assigning the class projects that this program is based       %%
%%%      on.  Starting in the fall of 2001 Matt Kontz will start his       %%
%%%      masters degree at Georgia Tech.                                   %% 
%%%                                                                        %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clf;clear all;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global slider1 slider2 slider3 slider4 slider5
global PushBut1 PushBut2 PushBut3 PushBut4 
global x1 y1 x2 y2 x3 y3 xt yt Pt			% position variable
global S1 S2 S3 S4 S5 S6						% position strings 
global pF1 pF2 pF3								% handles for fill
global dis Down          						% handles for text display
global C2 C3 Ct txA tx							% handles for the joint's circle
global J2 J3 Jt 									% handles for the joint's pluses
global L1 L2 L3 Link1 Link2 Link3			% Link matrices
global T1 T2 T3 STOP Chose						% input variables
global l1 l2 l3 rmax rmin Bmax				% constants
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fig=gcf;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Define Base	
angle=225:15:315;		% get rit of where links overlap
xa=1.125*cos(angle*pi/180);
ya=1.125*sin(angle*pi/180);
xb=[xa .875 .875 2.5  2.5 -2.5 -2.5 -.875 -.875];
yb=[ya -.707107 -1.25 -1.25 -2.875 -2.875 -1.25 -1.25 -.707107];
B=[xb' yb' zeros(size(xb))' ones(size(xb))']';  % Link3 matrix
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Link1 and Link2  
angle=265:-15:105;		% define an arc from 270 to 90 degree r=1.125
xa=1.125*cos(angle*pi/180);
ya=1.125*sin(angle*pi/180);
angle=150:15:210;		% get rit of where links overlap
xc=8.625+1.125*cos(angle*pi/180);
yc=1.125*sin(angle*pi/180);
xL1=[0 7.1875 7.1875 7.72978 xc 7.72978 7.1875 7.1875 0 xa 0];
yL1=[1.125 1.125 .875 .68133 yc -.68133 -.875 -1.125 -1.125 ya 1.125];
Link1=[xL1' yL1' zeros(size(xL1))' ones(size(xL1))']';  % Link1 matrix
Link2=Link1;				% Link1 and Link2 are the same
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Link3  
xL3=[0 2.875 3.125 3.125 4.875 6.625 6.6258 4.875 3.125 3.125 2.875 0 xa 0];
yL3=[1.125*ones(1,3) .875 .875 .25 -.25 -.875 -.875 -1.125*ones(1,3) ya 1.125];
Link3=[xL3' yL3' zeros(size(xL3))' ones(size(xL3))']';  % Link3 matrix
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
l1=8.625;				% distance between frame '1' and '2'
l2=l1;					% distance between frame '2' and '3'
l3=6.125;				% distance between frame '3' and 'tool'
rmax=l1+l2+l3;						% maximum distance between (0,0) and tool frame
rmin=(l2^2+(l1-l3)^2)^0.5;		% minimum distance between (0,0) and tool frame
Bmax=atan2(l1-l3,l2)+pi/2;
x0=0;						% x position of frame '0'	
y0=0;						% y position of frame '0'
x1=x0;					% x position of frame '1'	
y1=y0;					% y position of frame '1'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
r_i=rmax;				% initial radius
psi_i=90*pi/180;		% initial angle
T1=0;T2=0;T3=0;
forkin
r=rmax;
psi=pi/2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%           
tx(1)=text(29.7,-11,num2str(rmin),'HorizontalAlignment','right'); 
tx(2)=text(29.7,-4,'Radius','HorizontalAlignment','right');   
tx(3)=text(29.7,3,num2str(0.1*round(10*rmax)),'HorizontalAlignment','right');
tx(4)=text(29.7,11,'0^o','HorizontalAlignment','right'); 
tx(5)=text(29.7,18,'Angle','HorizontalAlignment','right');   
tx(6)=text(29.7,25,[num2str(180),'^o'],'HorizontalAlignment','right');
set(tx,'visible','off')			% sets label on slider and turns off
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
sr = ['r = get(gco,''Value'');' ...		% defines the new radius slider value
      'invkin(r,psi);' ...
      'forkin;' ...
      'setplot;'];% ...			% calls setplot to figure
slider4=uicontrol(fig,'Style','slider','Units','normalized', ...
   	'Position',[0.96 0.1 0.03 0.35],'min',rmin,'max',rmax, ...
   	'Value',r_i,'Callback',sr,'visible','off','BackgroundColor',[1 1 1]); 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
sp = ['psi_last=psi;' ...
   	'psi = get(gco,''Value'');' ...	% defines the new angle slider values
     	'if (psi_last>=pi/2 & psi<pi/2) | (psi_last<pi/2 & psi>=pi/2);' ...
      	'invkin(r,psi_last);' ...		% checks for cross over
        	'step=6;' ...
        	'T1_last=T1;' ...
  			'T2_last=T2;' ...
 	  		'T3_last=T3;' ...
      	'invkin(r,psi);' ...
      	'T1_goal=T1;' ...
      	'T2_goal=T2;' ...
 			'T3_goal=T3;' ...
  			'DeltaT1=(T1_goal-T1_last)/step;' ...
  			'DeltaT2=(T2_goal-T2_last)/step;' ...
   		'DeltaT3=(T3_goal-T3_last)/step;' ...
  			'for n=1:step;' ...		% animated robot from last to goal
     			'T1=T1_last+n*DeltaT1;' ...
     			'T2=T2_last+n*DeltaT2;' ...
     			'T3=T3_last+n*DeltaT3;' ...
       	 	'forkin;' ...
      	  	'setplot;' ...
     			'pause(0);' ...
  			'end;' ...
     	'else;' ...   
   		'invkin(r,psi);' ...
      	'forkin;' ...
      	'setplot;' ...
   	'end'];% ...			% calls setplot to figure
slider5=uicontrol(fig,'Style','slider','Units','normalized', ...
   	'Position',[0.96 0.55 0.03 0.35],'min',0,'max',pi, ...
      'Value',psi_i,'Callback',sp,'visible','off','BackgroundColor',[1 1 1]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
txA(1)=text(29,17.25,'-90','HorizontalAlignment','right'); 
txA(2)=text(29,22.25,'\theta_{3}','HorizontalAlignment','right');   
txA(3)=text(29,27.25,'90','HorizontalAlignment','right');
txA(4)=text(29,1.5,'-90','HorizontalAlignment','right'); 
txA(5)=text(29,6.5,'\theta_{2}','HorizontalAlignment','right');   
txA(6)=text(29,11.5,'90','HorizontalAlignment','right');
txA(7)=text(29,-14.25,'-90','HorizontalAlignment','right'); 
txA(8)=text(29,-9.25,'\theta_{1}','HorizontalAlignment','right');   
txA(9)=text(29,-4.25,'90','HorizontalAlignment','right');
set(txA,'visible','off')			% sets label on slider and turns off
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
s3 = ['T3 = get(gco,''Value'');' ...		% defines T3 as slider value
      'forkin;' ...
      'setplot;'];		% calls setplot to figure
slider3=uicontrol(fig,'Style','slider','Units','normalized', ...
   	'Position',[0.95 0.7 0.03 0.25],'min',-90,'max',90, ...
      'Value',0,'Callback',s3,'visible','off','BackgroundColor',[.2 .2 .8]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
s2 = ['T2 = get(gco,''Value'');' ...		% defines T2 as slider value
      'forkin;' ...
      'setplot;'];			% calls setplot to figure
slider2=uicontrol(fig,'Style','slider','Units','normalized', ...