set(L5, 'facec', [0.216,1,.583]);
set(L5, 'EdgeColor','none');
set(L6, 'facec', [1,1,0.255]);
set(L6, 'EdgeColor','none');
set(L7, 'facec', [0.306,0.733,1]);
set(L7, 'EdgeColor','none');
set(A1, 'facec', [.8,.8,.8],'FaceAlpha',.25);
set(A1, 'EdgeColor','none');
%
setappdata(0,'ThetaOld',[90,-90,-90,0,0,0]);
%
end
%%
function T = tmat(alpha, a, d, theta)
% tmat(alpha, a, d, theta) (T-Matrix used in Robotics)
% The homogeneous transformation called the "T-MATRIX"
% as used in the Kinematic Equations for robotic type
% systems (or equivalent).
%
% This is equation 3.6 in Craig's "Introduction to Robotics."
% alpha, a, d, theta are the Denavit-Hartenberg parameters.
%
% (NOTE: ALL ANGLES MUST BE IN DEGREES.)
%
alpha = alpha*pi/180; %Note: alpha is in radians.
theta = theta*pi/180; %Note: theta is in radians.
c = cos(theta);
s = sin(theta);
ca = cos(alpha);
sa = sin(alpha);
T = [c -s 0 a; s*ca c*ca -sa -sa*d; s*sa c*sa ca ca*d; 0 0 0 1];
end
%%
function del_app(varargin)
%This is the main figure window close function, to remove any
% app data that may be left due to using it for geometry.
%CloseRequestFcn
% here is the data to remove:
% Link1_data: [1x1 struct]
% Link2_data: [1x1 struct]
% Link3_data: [1x1 struct]
% Link4_data: [1x1 struct]
% Link5_data: [1x1 struct]
% Link6_data: [1x1 struct]
% Link7_data: [1x1 struct]
% Area_data: [1x1 struct]
% patch_h: [1x9 double]
% ThetaOld: [90 -182 -90 -106 80 106]
% xtrail: 0
% ytrail: 0
% ztrail: 0
% Now remove them.
rmappdata(0,'Link1_data');
rmappdata(0,'Link2_data');
rmappdata(0,'Link3_data');
rmappdata(0,'Link4_data');
rmappdata(0,'Link5_data');
rmappdata(0,'Link6_data');
rmappdata(0,'Link7_data');
rmappdata(0,'ThetaOld');
rmappdata(0,'Area_data');
rmappdata(0,'patch_h');
rmappdata(0,'xtrail');
rmappdata(0,'ytrail');
rmappdata(0,'ztrail');
delete(fig_1);
end
%%
function [hout,ax_out] = uibutton(varargin)
%uibutton: Create pushbutton with more flexible labeling than uicontrol.
% Usage:
% uibutton accepts all the same arguments as uicontrol except for the
% following property changes:
%
% Property Values
% ----------- ------------------------------------------------------
% Style 'pushbutton', 'togglebutton' or 'text', default =
% 'pushbutton'.
% String Same as for text() including cell array of strings and
% TeX or LaTeX interpretation.
% Interpreter 'tex', 'latex' or 'none', default = default for text()
%
% Syntax:
% handle = uibutton('PropertyName',PropertyValue,...)
% handle = uibutton(parent,'PropertyName',PropertyValue,...)
% [text_obj,axes_handle] = uibutton('Style','text',...
% 'PropertyName',PropertyValue,...)
%
% uibutton creates a temporary axes and text object containing the text to
% be displayed, captures the axes as an image, deletes the axes and then
% displays the image on the uicontrol. The handle to the uicontrol is
% returned. If you pass in a handle to an existing uicontol as the first
% argument then uibutton will use that uicontrol and not create a new one.
%
% If the Style is set to 'text' then the axes object is not deleted and the
% text object handle is returned (as well as the handle to the axes in a
% second output argument).
%
% See also UICONTROL.
% Version: 1.6, 20 April 2006
% Author: Douglas M. Schwarz
% Email: dmschwarz=ieee*org, dmschwarz=urgrad*rochester*edu
% Real_email = regexprep(Email,{'=','*'},{'@','.'})
% Detect if first argument is a uicontrol handle.
keep_handle = false;
if nargin > 0
h = varargin{1};
if isscalar(h) && ishandle(h) && strcmp(get(h,'Type'),'uicontrol')
keep_handle = true;
varargin(1) = [];
end
end
% Parse arguments looking for 'Interpreter' property. If found, note its
% value and then remove it from where it was found.
interp_value = get(0,'DefaultTextInterpreter');
arg = 1;
remove = [];
while arg <= length(varargin)
v = varargin{arg};
if isstruct(v)
fn = fieldnames(v);
for i = 1:length(fn)
if strncmpi(fn{i},'interpreter',length(fn{i}))
interp_value = v.(fn{i});
v = rmfield(v,fn{i});
end
end
varargin{arg} = v;
arg = arg + 1;
elseif ischar(v)
if strncmpi(v,'interpreter',length(v))
interp_value = varargin{arg+1};
remove = [remove,arg,arg+1];
end
arg = arg + 2;
elseif arg == 1 && isscalar(v) && ishandle(v) && ...
any(strcmp(get(h,'Type'),{'figure','uipanel'}))
arg = arg + 1;
else
error('Invalid property or uicontrol parent.')
end
end
varargin(remove) = [];
% Create uicontrol, get its properties then hide it.
if keep_handle
set(h,varargin{:})
else
h = uicontrol(varargin{:});
end
s = get(h);
if
any(strcmp(s.Style,{'pushbutton','togglebutton','text'}))
delete(h)
error('''Style'' must be pushbutton, togglebutton or text.')
end
set(h,'Visible','off')
% Create axes.
parent = get(h,'Parent');
ax = axes('Parent',parent,...
'Units',s.Units,...
'Position',s.Position,...
'XTick',[],'YTick',[],...
'XColor',s.BackgroundColor,...
'YColor',s.BackgroundColor,...
'Box','on',...
'Color',s.BackgroundColor);
% Adjust size of axes for best appearance.
set(ax,'Units','pixels')
pos = round(get(ax,'Position'));
if strcmp(s.Style,'text')
set(ax,'Position',pos + [0 1 -1 -1])
else
set(ax,'Position',pos + [4 4 -8 -8])
end
switch s.HorizontalAlignment
case 'left'
x = 0.0;
case 'center'
x = 0.5;
case 'right'
x = 1;
end
% Create text object.
text_obj = text('Parent',ax,...
'Position',[x,0.5],...
'String',s.String,...
'Interpreter',interp_value,...
'HorizontalAlignment',s.HorizontalAlignment,...
'VerticalAlignment','middle',...
'FontName',s.FontName,...
'FontSize',s.FontSize,...
'FontAngle',s.FontAngle,...
'FontWeight',s.FontWeight,...
'Color',s.ForegroundColor);
% If we are creating something that looks like a text uicontrol then we're
% all done and we return the text object and axes handles rather than a
% uicontrol handle.
if strcmp(s.Style,'text')
delete(h)
if nargout
hout = text_obj;
ax_out = ax;
end
return
end
% Capture image of axes and then delete the axes.
frame = getframe(ax);
delete(ax)
% Build RGB image, set background pixels to NaN and put it in 'CData' for
% the uicontrol.
if isempty(frame.colormap)
rgb = frame.cdata;
else
rgb = reshape(frame.colormap(frame.cdata,:),[pos([4,3]),3]);
end
size_rgb = size(rgb);
rgb = double(rgb)/255;
back = repmat(permute(s.BackgroundColor,[1 3 2]),size_rgb(1:2));
isback = all(rgb == back,3);
rgb(repmat(isback,[1 1 3])) = NaN;
set(h,'CData',rgb,'String','','Visible',s.Visible)
% Assign output argument if necessary.
if nargout
hout = h;
end
%%
end
end
demo = uicontrol(fig_1,'String','Demo','callback',@demo_button_press,...
'Position',[20 5 60 20]);
rnd_demo = uicontrol(fig_1,'String','Random Move','callback',@rnd_demo_button_press,...
'Position',[100 5 80 20]);
clr_trail = uicontrol(fig_1,'String','Clr Trail','callback',@clr_trail_button_press,...
'Position',[200 5 60 20]);
%
home = uicontrol(fig_1,'String','Home','callback',@home_button_press,...
'Position',[280 5 70 20]);
%
% Kinematics Panel
%
K_p = uipanel(fig_1,...
'units','pixels',...
'Position',[20 45 265 200],...
'Title','Kinematics','FontSize',11);
%
% Angle Range Default Name
% Theta 1: 320 (-160 to 160) 90 Waist Joint
% Theta 2: 220 (-110 to 110) -90 Shoulder Joint
% Theta 3: 270 (-135 to 135) -90 Elbow Joint
% Theta 4: 532 (-266 to 266) 0 Wrist Roll
% Theta 5: 200 (-100 to 100) 0 Wrist Bend
% Theta 6: 532 (-266 to 266) 0 Wrist Swivel
t1_home = 90; % offset to define the "home" position as UP.
t2_home = -90;
t3_home = -90;
LD = 105; % Left, used to set the GUI.
HT = 18; % Height
BT = 156; % Bottom
%% GUI buttons for Theta 1. pos is: [left bottom width height]
t1_slider = uicontrol(K_p,'style','slider',...
'Max',160,'Min',-160,'Value',0,...
'SliderStep',[0.05 0.2],...
'callback',@t1_slider_button_press,...
'Position',[LD BT 120 HT]);
t1_min = uicontrol(K_p,'style','text',...
'String','-160',...
'Position',[LD-30 BT+1 25 HT-4]); % L, from bottom, W, H
t1_max = uicontrol(K_p,'style','text',...
'String','+160',...
'Position',[LD+125 BT+1 25 HT-4]); % L, B, W, H
t1_text = uibutton(K_p,'style','text',... % Nice program Doug. Need this
'String','\theta_1',... % due to no TeX in uicontrols.
'Position',[LD-100 BT 20 HT]); % L, B, W, H
% t1_text = uicontrol(K_p,'style','text',... % when matlab fixes uicontrol
% 'String','t1',... % for TeX, then I can use this.
% 'Position',[LD-100 BT 20 HT]); % L, B, W, H
t1_edit = uicontrol(K_p,'style','edit',...
'String',0,...
'callback',@t1_edit_button_press,...
'Position',[LD-75 BT 30 HT]); % L, B, W, H
%
%% GUI buttons for Theta 2.
BT = 126; % Bottom
t2_slider = uicontrol(K_p,'style','slider',...
'Max',115,'Min',-115,'Value',0,... % Mech. stop limits !
'SliderStep',[0.05 0.2],...
'callback',@t2_slider_button_press,...
'Position',[LD BT 120 HT]);
t2_min = uicontrol(K_p,'style','text',...
'String','-110',...
'Position',[LD-30 BT+1 25 HT-4]); % L, from bottom, W, H
t2_max = uicontrol(K_p,'style','text',...
'String','+110',...
'Position',[LD+125 BT+1 25 HT-4]); % L, B, W, H
t2_text = uibutton(K_p,'style','text',...
'String','\theta_2',...
'Position',[LD-100 BT 20 HT]); % L, B, W, H
t2_edit = uicontrol(K_p,'style','edit',...
'String',0,...
'callback',@t2_edit_button_press,...
'Position',[LD-75 BT 30 HT]); % L, B, W, H
%
%% GUI buttons for Theta 3.
BT = 96; % Bottom
t3_slider = uicontrol(K_p,'style','slider',...
'Max',135,'Min',-135,'Value',0,...
'SliderStep',[0.05 0.2],...
'callback',@t3_slider_button_press,...
'Position',[LD BT 120 HT]);
t3_min = uicontrol(K_p,'style','text',...
'String','-135',...
'Position',[LD-30 BT+1 25 HT-4]); % L, from bottom, W, H
t3_max = uicontrol(K_p,'style','text',...
'String','+135',...
'Position',[LD+125 BT+1 25 HT-4]); % L, B, W, H
t3_text = uibutton(K_p,'style','text',...
'String','\theta_3',...
'Position',[LD-100 BT 20 HT]); % L, B, W, H
t3_edit = uicontrol(K_p,'style','edit',...
'String',0,...
'callback',@t3_edit_button_press,...
'Position',[LD-75 BT 30 HT]); % L, B, W, H
%
%% GUI buttons for Theta 4.
BT = 66; % Bottom
t4_slider = uicontrol(K_p,'style','slider',...
'Max',266,'Min',-266,'Value',0,...
'SliderStep',[0.05 0.2],...
'callback',@t4_slider_button_press,...
'Position',[LD BT 120 HT]);
t4_min = uicontrol(K_p,'style','text',...
'String','-266',...
'Position',[LD-30 BT+1 25 HT-4]); % L, from bottom, W, H
t4_max = uicontrol(K_p,'style','text',...
'String','+266',...
'Position',[LD+125 BT+1 25 HT-4]); % L, B, W, H
t4_text = uibutton(K_p,'style','text',...
'String','\theta_4',...
'Position',[LD-100 BT 20 HT]); % L, B, W, H
t4_edit = uicontrol(K_p,'style','edit',...
'String',0,...
'callback',@t4_edit_button_press,...
'Position',[LD-75 BT 30 HT]); % L, B, W, H
%
%% GUI buttons for Theta 5.
BT = 36; % Bottom
t5_slider = uicontrol(K_p,'style','slider',...
'Max',100,'Min',-100,'Value',0,...
'SliderStep',[0.05 0.2],...
'callback',@t5_slider_button_press,...
'Position',[LD BT 120 HT]);
t5_min = uicontrol(K_p,'style','text',...
'String','-100',...
'Position',[LD-30 BT+1 25 HT-4]); % L, from bottom, W, H
t5_max = uicontrol(K_p,'style','text',...
'String','+100',...
'Position',[LD+125 BT+1 25 HT-4]); % L, B, W, H
t5_text = uibutton(K_p,'style','text',...
'String','\theta_5',...
'Position',[LD-100 BT 20 HT]); % L, B, W, H
t5_edit = uicontrol(K_p,'style','edit',...
'String',0,...
'callback',@t5_edit_button_press,...
'Position',[LD-75 BT 30 HT]); % L, B, W, H
%
%% GUI buttons for Theta 6.
BT = 6; % Bottom
t6_slider = uicontrol(K_p,'style','slider',...
'Max',266,'Min',-266,'Value',0,...
'SliderStep',[0.05 0.2],...
'callback',@t6_slider_button_press,...
'Position',[LD BT 120 HT]);
t6_min = uicontrol(K_p,'style','text',...
'String','-266',...
'Position',[LD-30 BT+1 25 HT-4]); % L, from bottom, W, H
t6_max = uicontrol(K_p,'style','text',...
'String','+266',...
'Position',[LD+125 BT+1 25 HT-4]); % L, B, W, H
t6_text = uibutton(K_p,'style','text',...
'String','\theta_6',...
'Position',[LD-100 BT 20 HT]); % L, B, W, H
t6_edit = uicontrol(K_p,'style','edit',...
'String',0,...
'callback',@t6_edit_button_press,...
'Position',[LD-75 BT 30 HT]); % L, B, W, H
%
%% Slider for Theta 1 motion.
%
function t1_slider_button_press(h,dummy)
slider_value = round(get(h,'Value'));
set(t1_edit,'string',slider_value);
T_Old = getappdata(0,'ThetaOld');
t2old = T_Old(2); t3old = T_Old(3); t4old = T_Old(4);
t5old = T_Old(5); t6old = T_Old(6);
pumaANI(slider_value+t1_home,t2old,t3old,t4old,t5old,t6old,10,'n')
end
%
%% Slider for Theta 2 motion.
%
function t2_slider_button_press(h,dummy)
slider_value = round(get(h,'Value'));
set(t2_edit,'string',slider_value);
T_Old = getappdata(0,'ThetaOld');
t1old = T_Old(1); t3old = T_Old(3); t4old = T_Old(4);
t5old = T_Old(5); t6old = T_Old(6);
pumaANI(t1old,slider_value+t2_home,t3old,t4old,t5old,t6old,10,'n')
end
%
%% Slider for Theta 3 motion.
function t3_slider_button_press(h,dummy)
slider_value = round(get(h,'Value'));
set(t3_edit,'string',slider_value);
T_Old = getappdata(0,'ThetaOld');
t1old = T_Old(1); t2old = T_Old(2); t4old = T_Old(4);
t5old = T_Old(5); t6old = T_Old(6);
pumaANI(t1old,t2old,slider_value+t3_home,t4old,t5old,t6old,10,'n')
end
%
%% Slider for Theta 4 motion.
function t4_slider_button_press(h,dummy)
slider_value = round(get(h,'Value'));
set(t4_edit,'string',slider_value);
T_Old = getappdata(0,'ThetaOld');
t1old = T_Old(1); t2old = T_Old(2); t3old = T_Old(3);
t5old = T_Old(5); t6old = T_Old(6);
pumaANI(t1old,t2old,t3old,slider_value,t5old,t6old,10,'n')
end
%
%% Slider for Theta 5 motion.
function t5_slider_button_press(h,dummy)
slider_value = round(get(h,'Value'));
set(t5_edit,'string',slider_value);
T_Old = getappdata(0,'ThetaOld');
t1old = T_Old(1); t2old = T_Old(2); t3old = T_Old(3);
t4old = T_Old(4); t6old = T_Old(6);
pumaANI(t1old,t2old,t3old,t4old,slider_value,t6old,10,'n')
end
%
%% Slider for Theta 6 motion.
function t6_slider_button_press(h,dummy)
|
Скачать 337.85 Kb.