球杆系统-角度控制器设计
不同于普通的直流电机, 输入电压输出转速, 步进电机是输入脉冲, 输出角度变化, 换言之, 也可以是输入单位时间的脉冲, 输出单位时间的角度变化, 其实就是转速。 我们可以通过控制脉冲的频率控制电机转速, 通过脉冲的顺序控制电机的转向。makeblock的步进电机驱动替我完成了脉冲控制的部分, 我只需要提供一个PWM信号作为脉冲, 一个电平信号作为转向即可。 PWM的频率能够控制电机的转速关于PWM的频率变化, 需要通过配置寄存器来改变内部定时器的设定。在这里我使用了Arduino PWM library, 该库具有改变PWM频率的功能。该库提供了一个直接改变指定引脚方波频率的函数, 直接使用即可。
配置方法如下:
#include <;PWM.h>
int led = 11; // the pin that the LED is attached to
int32_t frequency = 10000; //frequency (in Hz)
void setup()
{
//initialize all timers except for 0, to save time keeping functions
InitTimersSafe();
bool success = SetPinFrequencySafe(led, frequency);
//if the pin frequency was set successfully, turn pin 13 on
if (success) {
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
}
}
步进电机驱动有两个控制口, 一个链接在D11上, 给pwm脉冲信号,另一个接在D12上, 给高低电平, 控制转向。 编写电机运动代码:
void run_motor(int speed_value){
if(speed_value >10000){
speed_value =10000;
}
if(speed_value <-10000){
speed_value =-10000;
}
if(speed_value >0){
digitalWrite(12,0);
while (digitalRead(12)==1){
digitalWrite(12,0);
}
analogWrite(11,128);
SetPinFrequencySafe(led,speed_value);
//digitalWrite(7,1);
}else if(speed_value <0){
digitalWrite(12,1);
while (digitalRead(12)==0){
digitalWrite(12,1);
}
analogWrite(11,128);
SetPinFrequencySafe(led,abs(speed_value));
//digitalWrite(7,1);
}else if(speed_value =0){
digitalWrite(12,1);
//while(digitalRead(12)==0);
analogWrite(11,0);
//SetPinFrequencySafe(led,abs(speed_value));
//digitalWrite(7,1);
}
}
这段函数的输入参数为设定速度, 实际上就是频率值。 根据实际测量, 当超过10000时, 电机就无法输出扭矩正常转动了, 故做一个饱和限制。
接下来检测一下旋转电位器的输出, 发现ad读数在363时横杆为水平状态。既然电机已经能够控制, 横杆的角度也可以检测, 接下来就该做一个控制器,让横杆按照期望的角度运动。控制框图如下:
figure 2.1 角度控制框图
使用最简单的反馈策略, 即控制什么反馈什么。 由于噪声和测量分辨率的限制,反馈回来的角度不可避免的具有误差。 反馈控制器采用PD, 可以避免系统超调震荡。 理论上说, 单纯的P控制即可, 并不应该产生震荡, 但实际上系统存在一定的惯性, 超调是不可避免的, 因此必须加入微分控制。 不加积分的原因是,被控系统本身含有积分环节, 能够避免稳态误差, 故不需要引入积分环节。 加入前馈的原因是, 前馈能够又快又好的跟踪变化的信号。
编写控制器的函数如下:
float position_loop(int set_point,int state,int velocity,float p,float i,float d){
float e,de;
static float sum_e;
float u;
e =set_point -state;
de =-velocity;
sum_e +=e;
u =e *p +sum_e *i +de *d;
returnu;
}
编写执行部分代码如下
filted_angle = mid1();
angle_speed = dirivative_of_angle(filted_angle);
u = 15*observed_speed+position_loop(363-desire_angle, filted_angle, angle_speed, 80, 0, 80);
run_motor(u);
以上功能代码执行效果如下:
figure 2.2 角度控制效果
可以看到, 当设定角度为一个恒定值时, 存在一些频率不高的抖动。 这个抖动来自于角度AD10位的分辨率。 假如使用12位的AD则抖动几乎不可见。 深入的说, 低速或稳态下检测的低分辨率和不连续导致了系统的震荡。 不过这种震荡的范围比较小, 只有0.35度。 假如换成12位AD,抖动的范围就会变成0.087度。接下来就要进行小球的位置控制了。此外还有一个小插曲, 角度传感器在电机通电时收到一些干扰, 导致角度检测有尖峰噪声, 我通过中值滤波将其滤去了。
说点什么