自控原理实验指导书
图4-12 固高科技球杆系统界面
在界面左边,可以观察控制的结果,如下图所示,
图4-13 运行结果显示图
在右边,可以在线更改参数
1) 更改小球目标位置,拖动小球目标位置滑块,到需要的位置,或是键入小球目标位置. 2) 更改PID控制参数,上下拖动Kp,Ki,Kd窗口内滑块,调整其参数,由于Kp,Ki,Kd都为
int型,所以只能为整数.(由于内部DSP乘法计算限制)
3) 点击”Break”可以暂停运动,点击”Continue”继续运行,点击”Axisoff”关闭电机。
附 程序说明(TML语言):
第一部分: 变量声明(int 为16位整型, long为32位带符号整型, fixed 为32位实型, 前16位
24
自控原理实验指导书
为整数部分,后16位为小数部分) 注:英文注释为程序自带注释
int Kp; //PID控制器比例系数 int Ki; //PID控制器积分系数 int Kd; //PID控制器微分系数 fixed BallRefPos; //小球目标位置 fixed FeedbackVol; //反馈电压 fixed BallActPos; //小球实际位置
fixed BallPosErrNew; //此采样周期中小球实际位置相对目标位置的位置误差 fixed BallPosErrOld; //上次采样周期中小球实际位置相对目标位置的位置误差 fixed BallPosErrDot; //小球运动速度
fixed PIDMotorPos; //PID比例部分引起的电机目标位置 fixed PIDErrDot; // PID微分部分引起的电机目标位置 fixed BallPosErrIntegral; //PID积分部分引起的电机目标位置 long MulResult; //乘积结果(用于IPM100内部DSP乘积运算) int MulTran; //乘积转移变量(用于IPM100内部DSP乘积运算) fixed PD_PID_Select; //PD PID控制器选择因子(用于分段PID控制) int MotorTPos; //电机目标位置
BallRefPos = 200; //Set 32 bit variable BallRefPos with value/32 bit variable 200 Kp = 4; //Set variable Kp with value/16 bit variable/label 4 Ki = 8; //Set variable Ki with value/16 bit variable/label 8 Kd = 4; //Set variable Kd with value/16 bit variable/label 4
第二部分: 主控制循环
loop:
FeedbackVol(H) = 0; //Set HIGH part of variable FeedbackVol with value/16 bit variable 0
FeedbackVol(L) = AD5; //Set LOW part of variable FeedbackVol with value/16 bit variable AD5 //通过AD5
读取电压反馈值,0-5V的电压被转化成0-1的小数
FeedBackVol*400 << 0; //Set P register with the product of variable FeedBackVol with value/variable 400
LEFT shifted with 0 bits.//电压反馈值乘以400(杆实际有效长度)得到小球实际位置
MulResult = PROD(L); //Set 32 bit variable MulResult with value/32 bit variable PROD(L) //乘积结果送入乘积寄存器
MulTran = MulResult(H); //Set variable MulTran with HIGH part of 32 bit variable MulResult BallActPos(H) = MulTran; //Set HIGH part of variable BallActPos with value/16 bit variable MulTran MulTran = MulResult(L); //Set variable MulTran with LOW part of 32 bit variable MulResult BallActPos(L) = MulTran; //Set LOW part of variable BallActPos with value/16 bit variable MulTran //48位的乘积结果转化成32位fixed实数型
BallPosErrNew = BallActPos; //Set 32 bit variable BallPosErrNew with value/32 bit variable BallActPos BallPosErrNew -= BallRefPos; //Substract from variable BallPosErrNew the value/variable BallRefPos //计算位置误差
BallPosErrDot = BallPosErrNew; //Set 32 bit variable BallPosErrDot with value/32 bit variable
BallPosErrNew
BallPosErrDot -= BallPosErrOld; //Substract from variable BallPosErrDot the value/variable BallPosErrOld
25
自控原理实验指导书
BallPosErrOld = BallPosErrNew; //Set 32 bit variable BallPosErrOld with value/32 bit variable
BallPosErrNew// 计算位置误差微分
BallPosErrDot*20 << 0; //Set P register with the product of variable BallPosErrDot with value/variable 20
LEFT shifted with 0 bits. //采样周期位50ms,转化单位 mm/采样周期 到 mm/s
MulResult = PROD; //Set 32 bit variable MulResult with value/32 bit variable PROD MulTran = MulResult(H); //Set variable MulTran with HIGH part of 32 bit variable MulResult
BallPosErrDot(H) = MulTran; //Set HIGH part of variable BallPosErrDot with value/16 bit variable MulTran MulTran = MulResult(L); //Set variable MulTran with LOW part of 32 bit variable MulResult
BallPosErrDot(L) = MulTran; //Set LOW part of variable BallPosErrDot with value/16 bit variable MulTran //48位的乘积结果转化成32位fixed实数型
PD_PID_Select = BallPosErrDot; //Set 32 bit variable PD_PID_Select with value/32 bit variable
BallPosErrDot //根据误差微分选择PID控制策略
GOTO BallPosErrDot_GT_0, PD_PID_Select, GT; //GOTO BallPosErrDot_GT_0 if PD_PID_Select > 0 PD_PID_Select += 50; //Add to variable PD_PID_Select the value/variable 50 GOTO PD_control, PD_PID_Select, LT; //GOTO PD_control if PD_PID_Select < 0 GOTO CheckPosErr; BallPosErrDot_GT_0:
PD_PID_Select -= 50; //Substract from variable PD_PID_Select the value/variable 50 GOTO PD_Control, PD_PID_Select, GT; //GOTO PD_Control if PD_PID_Select > 0 GOTO CheckPosErr; //当速度小于50mm/s时采用PID控制策略 CheckPosErr:
PD_PID_Select = BallPosErrNew; //Set 32 bit variable PD_PID_Select with value/32 bit variable BallPosErrNew
GOTO BallPosErr_GT_0, PD_PID_Select, GT; //GOTO BallPosErr_GT_0 if PD_PID_Select > 0 PD_PID_Select += 50; //Add to variable PD_PID_Select the value/variable 50 GOTO PD_control, PD_PID_Select, LT; //GOTO PD_control if PD_PID_Select < 0
BallPosErrIntegral += BallPosErrNew; //Add to variable BallPosErrIntegral the value/variable BallPosErrNew GOTO PID_control; BallPosErr_GT_0:
PD_PID_Select -= 50; //Substract from variable PD_PID_Select the value/variable 50 GOTO PD_Control, PD_PID_Select, GT; //GOTO PD_Control if PD_PID_Select > 0
BallPosErrIntegral += BallPosErrNew; //Add to variable BallPosErrIntegral the value/variable BallPosErrNew GOTO PID_Control; //当位置误差小于50mm时采用PID控制策略,注,只有同时满足上面两个条件
才采用PID控制,否则采用PD控制
PD_control:
BallPosErrIntegral = 0; //Set 32 bit variable BallPosErrIntegral with value/32 bit variable 0 //积分清零
PID_control: //比例控制作用
BallPosErrNew*Kp << 0; //Set P register with the product of variable BallPosErrNew with value/variable Kp LEFT shifted with 0 bits.
MulResult = PROD; //Set 32 bit variable MulResult with value/32 bit variable PROD MulTran = MulResult(H); //Set variable MulTran with HIGH part of 32 bit variable MulResult
PIDMotorPos(H) = MulTran; //Set HIGH part of variable PIDMotorPos with value/16 bit variable MulTran MulTran = MulResult(L); //Set variable MulTran with LOW part of 32 bit variable MulResult
26
自控原理实验指导书
PIDMotorPos(L) = MulTran; //Set LOW part of variable PIDMotorPos with value/16 bit variable MulTran //积分控制作用
BallPosErrIntegral >>= 6; //Shift RIGHT variable BallPosErrIntegral by 6 bits.
BallPosErrIntegral*Ki << 0; //Set P register with the product of variable BallPosErrIntegral with
value/variable Ki LEFT shifted with 0 bits.
MulResult = PROD; //Set 32 bit variable MulResult with value/32 bit variable PROD MulTran = MulResult(H); //Set variable MulTran with HIGH part of 32 bit variable MulResult PIDMotorPos(H) += MulTran; //Add to variable PIDMotorPos(H) the value/variable MulTran MulTran = MulResult(L); //Set variable MulTran with LOW part of 32 bit variable MulResult PIDMotorPos(L) += MulTran; //Add to variable PIDMotorPos(L) the value/variable MulTran BallPosErrIntegral <<= 6; //Shift LEFT variable BallPosErrIntegral by 6 bits. //微分控制作用
BallPosErrDot*Kd << 0; //Set P register with the product of variable BallPosErrDot with value/variable Kd
LEFT shifted with 0 bits.
MulResult = PROD; //Set 32 bit variable MulResult with value/32 bit variable PROD MulTran = MulResult(H); //Set variable MulTran with HIGH part of 32 bit variable MulResult PIDMotorPos(H) += MulTran; //Add to variable PIDMotorPos(H) the value/variable MulTran MulTran = MulResult(L); //Set variable MulTran with LOW part of 32 bit variable MulResult PIDMotorPos(L) += MulTran; //Add to variable PIDMotorPos(L) the value/variable MulTran //给定电机目标位置及运动模式
MotorTPos = PIDMotorPos(H); //Set variable MotorTPos with HIGH part of 32 bit variable PIDMotorPos CPOS = MotorTPos<<0; //Set 32 bit variable CPOS with 16 bit value of variable MotorTPos left shifted with
0bits
CACC = 1.27324; //Acceleration command for position profile //给定电机运动加速度 CSPD = 8.00000; //Speed command for position profile //给定电机运行速度 CPA; //Position command is Absolute MODE PP3; //Set Position Profile Mode 3 TUM1; //Set Target Update Mode 1 UPD; //Update immediate
//设定时针为10 个DSP周期即: 10*5ms=50ms !RT 10; //Set event if RelativeTime >= 10 WAIT!; //WAIT until event occurs GOTO loop; //主循环结束
__
常见故障及解决办法：
1． IPM智能伺服驱动器和计算机通讯不正常，显示Board not found！请参见IPM100SK用户
手册中通讯错误解决办法！
2． 电机不能正常运转，请确认电机和驱动器连接是否正常！
小球位置误差过大，可能的原因：a。PID参数太小，增大Kp；b。初始位置不正确，退出程序，重新进入。并确认在程序开始运行时候小球导轨基本上水平。
27
自控原理实验指导书
附录二 直线倒立摆系统使用说明
固高倒立摆系统实验控制软件工作在Windows 2000操作系统环境下，不支持Windows98和95。系统运行时需要MATLAB 6 Release 12.1以及SIMULINK 4.1支持。用户使用控制软件前需自行安装上述软件。
固高倒立摆系统MATLAB实验软件的安装
第一步：倒立摆系统控制软件以光盘的形式分发，请将光盘插入驱动器
第二步：在光盘上找到软件安装目录，双击“setup.exe”,启动安装程序。系统出现图4-1提示
第三步：安装程序需要知道MATLAB软件所在的软件根目录路径，点击“Browse ?”按钮，系统会现图4-2浏览对话框，找到MATLAB软件路径并选中。
第四步：点击确定，然后点击“setup”按钮，系统会出现进度框。软件自动安装于MATLAB\\Toolbox\\pendPCI目录下。如果此目录存在，安装可能失败。
第五步：安装完毕，可以启动SIMULINK使用本软件。
28