0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018
0019
0020
0021
0022
0023
0024
0025
0026
0027
0028
0029
0030
0031
0032
0033 function drivebot(a,b)
0034 bgcol = [135 206 250]/255;
0035
0036 if isstr(a)
0037
0038 name = a;
0039 j = b;
0040
0041 rh = findobj('Tag', name);
0042
0043 handles = get(gco, 'Userdata');
0044 scale = handles{3};
0045 for r=rh',
0046 rr = get(r, 'UserData');
0047 q = rr.q;
0048 if isempty(q),
0049 q = zeros(1,rr.n);
0050 end
0051 if gco == handles{1},
0052
0053 q(j) = get(gco, 'Value') / scale(j);
0054 set(handles{2}, 'String', num2str(scale(j)*q(j)));
0055 else
0056
0057 q(j) = str2num(get(gco, 'String')) / scale(j);
0058 set(handles{1}, 'Value', q(j));
0059 end
0060 rr.q = q;
0061 set(r, 'UserData', rr);
0062 plot(rr, q)
0063 end
0064
0065
0066 t6 = fkine(rr, q);
0067 h3 = get(findobj('Tag', 'T6'), 'UserData');
0068 for i=1:3,
0069 set(h3(i,1), 'String', sprintf('%.3f', t6(i,4)));
0070 set(h3(i,2), 'String', sprintf('%.3f', t6(i,3)));
0071 end
0072 else
0073
0074
0075 r = a;
0076 scale = ones(r.n,1);
0077
0078 n = r.n;
0079 width = 300;
0080 height = 40;
0081 minVal = -pi;
0082 maxVal = pi;
0083
0084 qlim = r.qlim;
0085 if isempty(qlim),
0086 qlim = [minVal*ones(r.n,1) maxVal*ones(r.n,1)];
0087 end
0088
0089 if nargin < 2,
0090 q = zeros(1,n);
0091 else
0092 if isstr(b),
0093 if strncmp(b, 'deg', 3),
0094 disp('** in degree mode')
0095 L = r.link;
0096 for i=1:r.n,
0097 if L{i}.sigma == 0,
0098 scale(i) = 180/pi;
0099 end
0100 end
0101 end
0102 else
0103 q = b;
0104 end
0105 end
0106 t6 = fkine(r, q);
0107 fig = figure('Units', 'pixels', ...
0108 'Position', [0 -height width height*(n+2)], ...
0109 'Color', bgcol);
0110 set(fig,'MenuBar','none')
0111 delete( get(fig, 'Children') )
0112
0113
0114
0115
0116 rh = findobj('Tag', r.name);
0117
0118
0119 if ~isempty(rh),
0120 rr = get(rh(1), 'UserData');
0121 if ~isempty(rr.q),
0122 q = rr.q;
0123 end
0124 end
0125
0126
0127
0128 for i=1:n,
0129 uicontrol(fig, 'Style', 'text', ...
0130 'Units', 'pixels', ...
0131 'BackgroundColor', bgcol, ...
0132 'Position', [0 height*(n-i) width*0.1 height*0.4], ...
0133 'String', sprintf('q%d', i));
0134
0135 h(i) = uicontrol(fig, 'Style', 'slider', ...
0136 'Units', 'pixels', ...
0137 'Position', [width*0.1 height*(n-i) width*0.7 height*0.4], ...
0138 'Min', scale(i)*qlim(i,1), ...
0139 'Max', scale(i)*qlim(i,2), ...
0140 'Value', scale(i)*q(i), ...
0141 'Tag', sprintf('Slider%d', i), ...
0142 'Callback', ['drivebot(''' r.name ''',' num2str(i) ')']);
0143
0144 h2(i) = uicontrol(fig, 'Style', 'edit', ...
0145 'Units', 'pixels', ...
0146 'Position', [width*0.8 height*(n-i-0.1) width*0.2 height*0.7], ...
0147 'String', num2str(scale(i)*q(i)), ...
0148 'Tag', sprintf('Edit%d', i), ...
0149 'Callback', ['drivebot(''' r.name ''',' num2str(i) ')']);
0150
0151
0152 handles = {h(i) h2(i) scale};
0153 set(h(i), 'Userdata', handles);
0154 set(h2(i), 'Userdata', handles);
0155 end
0156
0157 uicontrol(fig, 'Style', 'text', ...
0158 'Units', 'pixels', ...
0159 'FontSize', 20, ...
0160 'HorizontalAlignment', 'left', ...
0161 'Position', [0 height*(n+1) 0.8*width height], ...
0162 'BackgroundColor', 'white', ...
0163 'String', r.name);
0164
0165
0166 uicontrol(fig, 'Style', 'text', ...
0167 'Units', 'pixels', ...
0168 'BackgroundColor', bgcol, ...
0169 'Position', [0 height*(n+0.5) 0.06*width height/2], ...
0170 'BackgroundColor', 'yellow', ...
0171 'FontSize', 10, ...
0172 'HorizontalAlignment', 'left', ...
0173 'String', 'x:');
0174
0175 h3(1,1) = uicontrol(fig, 'Style', 'edit', ...
0176 'Units', 'pixels', ...
0177 'Position', [0.06*width height*(n+0.5) width*0.2 height*0.6], ...
0178 'String', sprintf('%.3f', t6(1,4)), ...
0179 'Tag', 'T6');
0180
0181
0182 uicontrol(fig, 'Style', 'text', ...
0183 'Units', 'pixels', ...
0184 'BackgroundColor', bgcol, ...
0185 'Position', [0.26*width height*(n+0.5) 0.06*width height/2], ...
0186 'BackgroundColor', 'yellow', ...
0187 'FontSize', 10, ...
0188 'HorizontalAlignment', 'left', ...
0189 'String', 'y:');
0190
0191 h3(2,1) = uicontrol(fig, 'Style', 'edit', ...
0192 'Units', 'pixels', ...
0193 'Position', [0.32*width height*(n+0.5) width*0.2 height*0.6], ...
0194 'String', sprintf('%.3f', t6(2,4)));
0195
0196
0197 uicontrol(fig, 'Style', 'text', ...
0198 'Units', 'pixels', ...
0199 'BackgroundColor', bgcol, ...
0200 'Position', [0.52*width height*(n+0.5) 0.06*width height/2], ...
0201 'BackgroundColor', 'yellow', ...
0202 'FontSize', 10, ...
0203 'HorizontalAlignment', 'left', ...
0204 'String', 'z:');
0205
0206 h3(3,1) = uicontrol(fig, 'Style', 'edit', ...
0207 'Units', 'pixels', ...
0208 'Position', [0.58*width height*(n+0.5) width*0.2 height*0.6], ...
0209 'String', sprintf('%.3f', t6(3,4)));
0210
0211
0212 uicontrol(fig, 'Style', 'text', ...
0213 'Units', 'pixels', ...
0214 'BackgroundColor', bgcol, ...
0215 'Position', [0 height*(n) 0.06*width height/2], ...
0216 'BackgroundColor', 'yellow', ...
0217 'FontSize', 10, ...
0218 'HorizontalAlignment', 'left', ...
0219 'String', 'ax:');
0220
0221 h3(1,2) = uicontrol(fig, 'Style', 'edit', ...
0222 'Units', 'pixels', ...
0223 'Position', [0.06*width height*(n) width*0.2 height*0.6], ...
0224 'String', sprintf('%.3f', t6(1,3)));
0225
0226
0227 uicontrol(fig, 'Style', 'text', ...
0228 'Units', 'pixels', ...
0229 'BackgroundColor', bgcol, ...
0230 'Position', [0.26*width height*(n) 0.06*width height/2], ...
0231 'BackgroundColor', 'yellow', ...
0232 'FontSize', 10, ...
0233 'HorizontalAlignment', 'left', ...
0234 'String', 'ay:');
0235
0236 h3(2,2) = uicontrol(fig, 'Style', 'edit', ...
0237 'Units', 'pixels', ...
0238 'Position', [0.32*width height*(n) width*0.2 height*0.6], ...
0239 'String', sprintf('%.3f', t6(2,3)));
0240
0241
0242 uicontrol(fig, 'Style', 'text', ...
0243 'Units', 'pixels', ...
0244 'BackgroundColor', bgcol, ...
0245 'Position', [0.52*width height*(n) 0.06*width height/2], ...
0246 'BackgroundColor', 'yellow', ...
0247 'FontSize', 10, ...
0248 'HorizontalAlignment', 'left', ...
0249 'String', 'az:');
0250
0251 h3(3,2) = uicontrol(fig, 'Style', 'edit', ...
0252 'Units', 'pixels', ...
0253 'Position', [0.58*width height*(n) width*0.2 height*0.6], ...
0254 'String', sprintf('%.3f', t6(3,3)));
0255
0256
0257 set(h3(1,1), 'Userdata', h3);
0258 uicontrol(fig, 'Style', 'pushbutton', ...
0259 'Units', 'pixels', ...
0260 'FontSize', 16, ...
0261 'Position', [0.8*width height*n 0.2*width 2*height], ...
0262 'CallBack', 'delete(gcf)', ...
0263 'BackgroundColor', 'red', ...
0264 'String', 'Quit');
0265
0266
0267 if isempty(rh),
0268 figure
0269 plot(r, q);
0270 end
0271 end