前言：续写一知半解系列，在上一篇中相信对大家对GUI的控件操作和与Simulink数据传递比较熟悉了。这篇主要解决下上次留下的问题。主要包括 ：（1）GUI界面其他控件说明。包括运行时间，工具箱机器人显示和末端位姿计算显示，利用工具箱逆解函数ikine对末端进行操作。（2）PID参数的调节。
1、GUI界面其他控件说明
1）GUI显示运行时间，需要一个静态文本和可编辑文本
a、在guide界面拖入静态文本用于运行时间的标识，进行简单设置，一般设置如下。因为静态文本只作为标识，所以这里不需要对其进行回调函数编写。
b、在guide界面拖入可编辑文本用于当前时间的运行，进行简单设置如下。
注意这里的标签Tag自定义为Time，方便后面对句柄对象进行操作。由于运行时间是在运行GUI就开始显示。所以需要在GUI自动生成的OpeningFcn中编写代码用于时间的触发；并编写函数disptime用于时间显示，如下：
OpeningFcn函数：
function PUMA560_OpeningFcn(hObject, eventdata, handles, varargin)
clc
handles.output = hObject;
%% 运行时间
h=timer; %定时器
handles.T=h; %将定时器放到全局变量中
%set(handles.T,'ExecutionMode','singleShot'); %定时器只执行一次，定一次时。
set(handles.T,'ExecutionMode','fixedRate'); %定时器，循环执行，循环定时。
set(handles.T,'Period',1); %定时器，定时间隔 1秒
set(handles.T,'TimerFcn',{@disptime,handles}); %定时器，定时会触发 TimerFcn 函数
%定时函数(TimerFcn)触发用户自定义的函数(disptime函数)
start(handles.T); %开启定时器
%% 运行立即显示工具箱机器人模型
axes(handles.plot)%指定显示区域
mdl_puma560 %导入机器人模型
assignin('base','Robot',p560);%将p560赋给基础工作空间中的Robot变量
Robot= evalin('base','Robot');%将基础工作空间中的变量指定到GUI中，方便数据的共享
view(3) %三维视角
Robot.plot([0 0 0 0 0 0]);%显示机器人构型为[0 0 0 0 0 0]
guidata(hObject, handles);disptime函数：
function disptime(hObject, eventdata, handles)
% 自定义的函数，将edit控件的内容改成当前时间。
set(handles.Time,'String',datestr(now)); % 将edit控件的内容改成当前时间。2）GUI显示机器人工具箱模型
机器人显示在坐标区控件上，控件大小调节到合适大小，双击修改打开属性检测器，这里只修改标签Tag为plot（命名自定，最好是有意义的名字，方便操作）。
由于一运行GUI机器人模型就显示在坐标区，所以初始的显示代码同样写在了OpeningFcn函数中,在 1）中的OpeningFcn函数中可查。对应代码axes(handles.plot)%指定显示区域
3）末端位姿计算显示
需要控件和运行时间的控件一样，静态文本和可编辑文本，静态文本用于标识，可编辑文本用于计算结果的显示，过程大致如下。
如关节1滑块移动后对机器人单关节运动和末端位姿计算显示代码如下【其他同】
function Joint1_Callback(hObject, eventdata, handles)
ModelName = 'TestPuma560';
%get the angle
theta1=get(handles.Joint1,'value');
set(handles.edit1,'string',num2str(theta1));
set_param([ModelName '/Slider Gain'],'Gain',num2str(theta1));
theta2=get(handles.Joint2,'value');
set(handles.edit2,'string',num2str(theta2));
theta3=get(handles.Joint3,'value');
set(handles.edit3,'string',num2str(theta3));
theta4=get(handles.Joint4,'value');
set(handles.edit4,'string',num2str(theta4));
theta5=get(handles.Joint5,'value');
set(handles.edit5,'string',num2str(theta5));
theta6=get(handles.Joint6,'value');
set(handles.edit6,'string',num2str(theta6));
%Data transmission to simulink
set_param([ModelName '/Slider Gain1'],'Gain',num2str(theta2));
set_param([ModelName '/Slider Gain2'],'Gain',num2str(theta3));
set_param([ModelName '/Slider Gain3'],'Gain',num2str(theta4));
set_param([ModelName '/Slider Gain5'],'Gain',num2str(theta5));
set_param([ModelName '/Slider Gain6'],'Gain',num2str(theta6));
%Forword kinematics calculation
rpy=zeros(1,3);
a2 = 0.4318;
a3 = 0.0203;
d3 = 0.1501;
d4 = 0.4318;
t1=theta1;t2=theta2; t3=theta3;
t4=theta4; t5=theta5; t6=theta6;
T_01 = tmat(0, 0, 0, t1);
T_12 = tmat(90, 0, 0, t2);
T_23 = tmat(0, a2, d3, t3);
T_34 = tmat(-90, a3, d4, t4);
T_45 = tmat(90, 0, 0, t5);
T_56 = tmat(-90, 0, 0, t6);
% Each link fram to base frame transformation
T_02 = T_01*T_12;
T_03 = T_02*T_23;
T_04 = T_03*T_34;
T_05 = T_04*T_45;
T_06 = T_05*T_56;
X=T_06(1,4);Y=T_06(2,4);Z=T_06(3,4);%position
R=T_06;
if abs(abs(R(1,3)) - 1) < eps % when |R13| == 1
% singularity
rpy(1) = 0; % roll is zero
if R(1,3) > 0
rpy(3) = atan2( R(3,2), R(2,2)); % R+Y
else
rpy(3) = -atan2( R(2,1), R(3,1)); % R-Y
end
rpy(2) = asin(R(1,3));
else
rpy(1) = -atan2(R(1,2), R(1,1));
rpy(3) = -atan2(R(2,3), R(3,3));
rpy(2) = atan(R(1,3)*cos(rpy(1))/R(1,1));
end
RPY=rpy*180/pi;
Rall=round(RPY(1));Pitch=round(RPY(2));Yaw=round(RPY(3));
set(handles.X,'string',num2str(X));
set(handles.Y,'string',num2str(Y));
set(handles.Z,'string',num2str(Z));
set(handles.R,'string',num2str(Rall));
set(handles.P,'string',num2str(Pitch));
set(handles.Yr,'string',num2str(Yaw));
q=[theta1 theta2 theta3 theta4 theta5 theta6];
Robot= evalin('base','Robot');
handles.plot;
Robot.plot(q*pi/180);
% Robot.plot3d(q*pi/180, 'view',[-138 8]);
T=Robot.fkine(q*pi/180);
hold on
plot3(T.t(1,1),T.t(2,1),T.t(3,1),'.','LineWidth',3,'color','r');上面的正运动学计算末端位姿调用的tmat函数为：
function T = tmat(alpha, a, d, theta)%改进DH方法，坐标变换过程看出Craig
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];4）利用工具箱逆解函数ikine对末端进行笛卡尔空间操作
这里选用控件为静态文本和按钮。静态文本依然只作为标识，按钮按下作为末端位置矢量的输入，通过逆解得各关节角度，将关节角度再传递给机器人。
按钮的回调函数基本相同，下面以改变机器人末端X为列。
X-按钮回调代码：
function pushbutton3_Callback(hObject, eventdata, handles)
Robot= evalin('base','Robot');
theta1=get(handles.Joint1,'value');
theta2=get(handles.Joint2,'value');
theta3=get(handles.Joint3,'value');
theta4=get(handles.Joint4,'value');
theta5=get(handles.Joint5,'value');
theta6=get(handles.Joint6,'value');
q0=[theta1 theta2 theta3 theta4 theta5 theta6];
T0=Robot.fkine(q0*pi/180);R=[T0.n T0.o T0.a];%直接调用正运动学函数
numMats = size(R,3);H = zeros(4,4,numMats,'like',R);
H(1:3,1:3,:) = R;
H(4,4,:) = ones(1,1,numMats,'like',R);
H(1:3,4)=T0.t;%SE3 xyzMovement To Homogeneous matrix
Hinit=H;
offset=str2double(get(handles.MoveAccuracy,'string'));%运动的步长，精度
H(1:1 ,4:4)=Hinit(13)+offset;%以当前末端为参考进行末端运动
%Hinit(13)为矩阵第13个位置上的数，在齐次矩阵中为X坐标。
Hmove=H;
Q=Robot.ikine(Hmove);%调用逆运动学函数得到各关节值
handles.plot;
Robot.plot(Q);%显示末端运动
theta1=Q(1)*180/pi;theta2=Q(2)*180/pi;theta3=Q(3)*180/pi;
theta4=Q(4)*180/pi;theta5=Q(5)*180/pi;theta6=Q(6)*180/pi;
%设置滑块变动和对应角度显示
set(handles.Joint1,'value',theta1);set(handles.Joint2,'value',theta2);
set(handles.Joint3,'value',theta3); set(handles.Joint4,'value',theta4);
set(handles.Joint5,'value',theta5); set(handles.Joint6,'value',theta6);
set(handles.edit1,'string',num2str(theta1)); set(handles.edit2,'string',num2str(theta2));
set(handles.edit3,'string',num2str(theta3)); set(handles.edit4,'string',num2str(theta4));
set(handles.edit5,'string',num2str(theta5)); set(handles.edit6,'string',num2str(theta6));
%对Simulink的操作
ModelName = 'TestPuma560';
set_param([ModelName '/Slider Gain'],'Gain',num2str(theta1));
set_param([ModelName '/Slider Gain1'],'Gain',num2str(theta2));
set_param([ModelName '/Slider Gain2'],'Gain',num2str(theta3));
set_param([ModelName '/Slider Gain3'],'Gain',num2str(theta4));
set_param([ModelName '/Slider Gain5'],'Gain',num2str(theta5));
set_param([ModelName '/Slider Gain6'],'Gain',num2str(theta6));
%显示末端运动轨迹，X为红色
T=Robot.fkine(Q);
hold on
plot3(T.t(1,1),T.t(2,1),T.t(3,1),'.','LineWidth',3,'color','r');
X=T.t(1,1);Y=T.t(2,1);Z=T.t(3,1);
set(handles.X,'string',num2str(X));
set(handles.Y,'string',num2str(Y));
set(handles.Z,'string',num2str(Z));对逆运动学的控制在GUI中设置了末端位置矢量的变化步长，控件选择使用一个静态文本作为标识，静态文本拖入修改名字即可。可编辑文本可在运行时修改数据，并在位置按钮回调中获得。上面代码中的offset=str2double(get(handles.MoveAccuracy,’string’));可编辑文本修改如下：
效果：（这里只对位置矢量进行控制，若要对姿态进行控制可以添加控件按同样的方式完成）
2、PID参数的调节
双击打开PID控制器，默认参数如下。
在这种情况下用一个正弦波模块替换关节1的角度输入Sider Gian模块进行测试。
使用Scope示波器对其进行角度跟踪情况如下，很明显存在误差：
因此对PID参数进行调节。这里使用PID自动参数调节工具。先运行一次模型，然后双击PID控制模块点击Tune按钮。
调节下面的响应时间和鲁棒性（超调量），实线为调节后的响应会自动更新。
得到一个较满意的响应后，我们选择update，更新PID参数到simulink中。
如下PID参数自动生成之后，点击应用。
现在运行模型。可以看到关节1的角度跟踪情况变好了很多，误差已经很小了，说明关节1的PID参数调节就完成了，其他关节类似的方式。
到目前为止，我们将一知半解4里面留下的问题基本解决了。
后记：作者能力有限，如果有错误的地方请大家及时的指出。
（1）PID参数调节相关可参考：https://zhuanlan.zhihu.com/p/74131690
https://zhuanlan.zhihu.com/p/139945063
书籍：《先进PID控制MATLAB仿真 》刘金琨
（2）非guide方式的GUI设计学习可以参考：https://www.bilibili.com/video/BV1mK411p7c3
（3）GUI设计参考书籍：《MATLAB GUI设计入门与实战》
（4）机器人工具箱Robotics Toolbox学习可参考：https://www.bilibili.com/video/BV1MK4y1k7je
—以上
