SOPC之NIOS Ⅱ实现电机转速PID控制
通过FPGA开发板上的NIOS Ⅱ搭建电机控制的硬件平台，包括电机正反转、编码器的读取，再通过软件部分实现PID算法对电机速度进行控制，使其能够渐近设定的编码器目标值。
一、PID算法
PID算法（Proportional-Integral-Derivative Algorithm）是一种经典的控制算法，用于实现闭环控制系统中的自动控制，旨在使被控制系统的输出尽可能接近期望值。
PID算法由三个部分组成：比例（Proportional）、积分（Integral）和微分（Derivative）。这三个部分分别对应了误差的当前值、累积值和变化率。PID算法根据这些部分的加权和来计算控制输出，以实现对系统的稳定、快速、精确的控制。
- 比例(P)部分：比例控制是根据当前的误差值来调整控制输出。如果误差较大，比例部分的输出也会较大，从而更快地减小误差。这有助于系统快速接近期望值，但可能导致震荡和过冲。
- 积分(I)部分：积分控制用于消除系统存在的稳态误差。它考虑误差的累积值，如果误差持续存在，积分部分的输出会逐渐增加，从而逐步减小稳态误差。然而，过大的积分作用可能导致系统响应过于缓慢或产生震荡。
- 微分(D)部分：微分控制用于预测误差的未来变化趋势。通过考虑误差的变化率，微分部分可以抑制系统的过冲和震荡。但过大的微分作用可能引起噪音的放大。
PID算法的参数调节是一个复杂的过程，需要根据被控制系统的特性和性能要求进行调试。不同的应用和系统可能需要不同的PID参数配置，以达到最佳的控制效果。
PID算法详细内容和调试方法可参考：
PID参数解析+调参经验笔记（经验法）_pid调参_Xuan-ZY的博客-CSDN博客
二、硬件设计
硬件部分主要由两部分组成，卡尔曼滤波和NIOS Ⅱ系统
2.1 卡尔曼滤波
卡尔曼滤波主要是为了对霍尔传感器输出的方波进行滤波操作
reg [15:0] filterClockDivider; // 过滤器时钟分频器
reg filterClock; // 过滤器时钟信号
// 时钟设置
parameter ClockFrequency = 50000000; // 时钟频率50MHz
parameter FilterFrequency = 15000; // 滤波器频率15KHz
/ 过滤器时钟
always @(posedge Clock or negedge Reset)
begin
if (!Reset)
begin
filterClock <= 0; // 复位时，过滤器时钟为低电平
filterClockDivider <= 0; // 复位时，分频器清零
end
else
begin
if (filterClockDivider < (ClockFrequency / FilterFrequency / 8))
filterClockDivider <= filterClockDivider + 1; // 分频计数增加
else
begin
filterClockDivider <= 0; // 分频计数清零
filterClock <= ~filterClock; // 过滤器时钟翻转
end
end
end
always @(posedge filterClock or negedge Reset)
begin
if (!Reset)
begin
Output_A <= 0; // 复位时，输出信号 A 为低电平
end
else
begin
if (Input_A)
Output_A <= 1; // 如果输入信号 A 为高，输出信号 A 为高
else
Output_A <= 0; // 否则输出信号 A 为低
end
end
always @(posedge filterClock or negedge Reset)
begin
if (!Reset)
begin
Output_B <= 0; // 复位时，输出信号 B 为低电平
end
else
begin
if (Input_B)
Output_B <= 1; // 如果输入信号 B 为高，输出信号 B 为高
else
Output_B <= 0; // 否则输出信号 B 为低
end
end
endmodule3.1 NIOS Ⅱ系统
NIOS Ⅱ中包含时钟CLK、锁相环PLL、NIOS Ⅱ软核处理器、片上存储onchip_ram、System ID、串行通信jtag_uart，以及自定义组件电机控制Motor_PWM、编码器测量Motor_measure
3.1.1 电机控制
一般的FPGA是无法驱动电机的，因此需要电机驱动芯片控制DC电机，与FPGA相连接的控制信号有IN1/IN2/PWM，通过IN1/IN2去控制电机的方向与停止，通过PWM去控制电机的转速
`define REGISTER_TOTAL_DUR 2'd0
`define REGISTER_HIGH_DUR 2'd1
`define REGISTER_CONTROL 2'd2
reg motor_movement; // 电机运动，1为开始、0为停止
reg motor_direction; // 电机转向，1为向前、0为向后
reg motor_fast_decay; // 电机减速，1为快制动、0为慢制动
always @(posedge clock or negedge reset_n)
begin
if (~reset_n)
begin
// PWM
high_dur <= 0;
total_dur <= 0;
// MOTOR
motor_movement <= 1'b0;
motor_direction <= 1'b1;
motor_fast_decay <= 1'b1;
end
else if (select_cs && (select_address == `REGISTER_CONTROL))
begin
if (select_write)
{motor_fast_decay, motor_direction, motor_movement} <= select_writedata[2:0];
else if (select_read)
select_readdata <= {29'b0, motor_fast_decay, motor_direction, motor_movement};
end
else if (select_cs & select_write)
begin
if (select_address == `REGISTER_TOTAL_DUR)
total_dur <= select_writedata;
else if (select_address == `REGISTER_HIGH_DUR)
high_dur <= select_writedata;
end
else if (select_cs & select_read)
begin
if (select_address == `REGISTER_TOTAL_DUR)
select_readdata <= total_dur;
else if (select_address == `REGISTER_HIGH_DUR)
select_readdata <= high_dur;
end
end
// 方向控制
always @(*)
begin
if (motor_fast_decay)
begin
// 急刹车
if (motor_movement)
begin
if (motor_direction)
{DC_Motor_IN2, DC_Motor_IN1, PWM} <= {1'b1, 1'b0, PWM_OUT};
else
{DC_Motor_IN2, DC_Motor_IN1, PWM} <= {1'b0, 1'b1, PWM_OUT};
end
else
{DC_Motor_IN2, DC_Motor_IN1, PWM} <= {1'b1, 1'b1, 1'b0};
end
else
begin
// 慢刹车
if (motor_movement)
begin
if (motor_direction)
{DC_Motor_IN2, DC_Motor_IN1, PWM} <= {1'b1, 1'b0, PWM_OUT};
else
{DC_Motor_IN2, DC_Motor_IN1, PWM} <= {1'b0, 1'b1, PWM_OUT};
end
else
{DC_Motor_IN2, DC_Motor_IN1, PWM} <= {1'b0, 1'b0, 1'b0};
end
end
// PWM 转速控制
reg PWM_OUT;
reg [31:0] total_dur; // 总持续时间
reg [31:0] high_dur; // 高位时间，决定电机转速，控制 PWM 占空比，值越高，占空比越大，转速越快
reg [31:0] tick; // 计数器
always @(posedge clock or negedge reset_n)
begin
if (~reset_n)
begin
tick <= 1;
end
else if (tick >= total_dur)
begin
tick <= 1;
end
else
tick <= tick + 1;
end
always @(posedge clock)
begin
PWM_OUT <= (tick <= high_dur) ? 1'b1 : 1'b0;
end
endmodule3.1.2 电机监测
电机转动带动磁盘经过霍尔传感器，磁力的变化让霍尔效应传感器产生霍尔效应电压，经过数字电路处理产生方波，两个位置不同的霍尔效应传感器输出两个相位不同的方波(PhaseA和Phase B)。磁盘在转动时，先被感应的传感器会先输出方波，另一个传感器输出会有延迟，所以两个方波的相位有所不同。由此可以通过方波相位领先计算电机的方向。根据输出的脉冲数，可以计算电机转速。
首先计算电机的方向
reg DO_PULSE; //用于存储输出的电机脉冲信号
wire PULSE_XOR; //用于存储PHASE_A和PHASE_B进行异或结果
reg PULSE_XOR_PREVIOUS; //上一次的PULSE_XOR值
reg DIRECTION; //用于存储电机方向信号
reg DIRECT_PATCH; //用于存储DIRECT异或PHASE_A后取反的结果
//解码方向信号
always @(posedge DI_PHASE_A) DIRECTION <= DI_PHASE_B; //当有DI_PHASE_A的上升沿，将DI_PHASE_B的值赋给DIRECTION
always @(posedge DI_PHASE_B) DIRECT_PATCH <= ~(DIRECTION ^ DI_PHASE_A); //当有DI_PHASE_B的上升沿，将DIRECT和DI_PHASE_A进行异或后取反赋值给DIRECT_PATCH
assign DO_DIRECT = DIRECTION | DIRECT_PATCH; //将DIRECTION和DIRECT_PATCH进行与运算
//解码脉冲信号
assign PULSE_XOR = DI_PHASE_A ^ DI_PHASE_B;
always @(posedge DI_SYSCLK)
begin
if(PULSE_XOR != PULSE_XOR_PREVIOUS)
begin
DO_PULSE <= 1'b1;
PULSE_XOR_PREVIOUS <= PULSE_XOR;
end
else begin
DO_PULSE <= 1'b0;
end
end检测编码器的值
always @(posedge clock or negedge reset_n)
begin
if(~reset_n) //当复位有效将counter_threshold和counter_enable置为0
begin
counter_threshold <= 0;
counter_enable <= 0;
end
else if (select_chip_enable && select_write) //当select_chip_enable和select_write有效，即写有效
begin
if(select_register_address == `COUNTER_ENABLE)
counter_enable <= select_write_data;
end
else if(select_chip_enable && select_read)
begin //当读有效就读取当前counter数值
if(select_register_address == `COUNTER_READ)
select_read_data <= pulse_counter;
end
end
always @(posedge clock)
begin
if(select_chip_enable && select_write && select_register_address == `COUNTER_WRITE)
pulse_counter <= select_write_data[15:0];
else if(counter_enable && motor_pulse) //当计数使能和电机脉冲同时有效
begin
if(motor_direction) //如果电机正转
begin
if(pulse_counter < 16'hffff)
pulse_counter <= pulse_counter + 1; //counter随电机传回的脉冲数累加
end
else if(!motor_direction) //如果电机反转
begin
if(pulse_counter > 0)
pulse_counter <= pulse_counter - 1; //counter随着电机传回的脉冲数递减
end
else
pulse_counter <= 0;
end
end 三、软件设计
3.1 Motor控制
电机控制部分由Motor.h和Motor.cpp组成，对相关函数进行声明和定义
首先在构造函数中传入电机的地址和测量寄存器地址
Motor::Motor(int Add,int MeasureAdd):
motor_BaseAddress(Add),
measure_Address(MeasureAdd),
CycleWidthMini(CYCLE_WIDTH_MINI), //电机的最小PWM周期宽度值
CycleWidthMaxi(CYCLE_WIDTH_MAX) //电机的最大PWM周期宽度值
{
//将REG_TOTAL_DUR寄存器（0）设置为常量值CYCLE_WIDTH，设置电机的初始PWM周期宽度
IOWR(motor_BaseAddress, REG_TOTAL_DUR, CYCLE_WIDTH);
}控制电机的启动、停止以及方向
//启动电机
void Motor::StartMotor(void){
int currentStatus, updatedControl;
currentStatus = IORD(motor_BaseAddress, STATUS_REG); //从STATUS_REG寄存器读取当前状态
updatedControl = currentStatus | MOTOR_RUN_FLAG; //设置MOTOR_RUN_FLAG
IOWR(motor_BaseAddress, STATUS_REG, updatedControl); //然后将更新后的控制值写回寄存器
IOWR(measure_Address, measure_count_enable_reg, 0x01); //将0x01写入测量设备的measure_cnt_enable_reg 寄存器来启用测量计数器
}
//停止电机
void Motor::StopMotor(void){
int currentStatus, updatedControl;
currentStatus = IORD(motor_BaseAddress, STATUS_REG); //从STATUS_REG寄存器读取当前状态
updatedControl = currentStatus & (~MOTOR_RUN_FLAG); //清除MOTOR_RUN_FLAG
IOWR(motor_BaseAddress, STATUS_REG, updatedControl); //将更新后的控制值写回寄存器
IOWR(measure_Address, measure_count_enable_reg, 0x00); //通过将0写入测量设备的 measure_count_enable_reg 寄存器来禁用测量计数器
}
//设置电机方向
void Motor::SetMotorDirection(bool forwardDirection){
int currentStatus, updatedControl;
currentStatus = IORD(motor_BaseAddress, STATUS_REG); //从STATUS_REG寄存器中读取当前状态
if (forwardDirection) //根据forwardDirection参数修改方向控制
updatedControl = currentStatus | MOTOR_FORWARD_FLAG;
else
updatedControl = currentStatus & ~MOTOR_FORWARD_FLAG;
IOWR(motor_BaseAddress, STATUS_REG, updatedControl); //将更新后的值写回STATUS_REG寄存器中
}
设置电机转动的速度
//设置电机速度
void Motor::SetMotorSpeed(float desiredSpeed) {
int speedParameter = 0;
if (desiredSpeed < -100.0) //确保输入速度在有效范围内
desiredSpeed = -100;
else if (desiredSpeed > 100.0)
desiredSpeed = 100.0;
if (desiredSpeed != 0.0) { //根据输入速度计算适当的PWM值
speedParameter = motor_CycleWidth_Min + (int)(fabs(desiredSpeed) * (float)(motor_CycleWidth_Max - motor_CycleWidth_Min) / 100.0);
}
IOWR(motor_BaseAddress, REG_HIGH_DURATION, speedParameter); //将计算得到的PWM值写入REG_HIGH_DURATION寄存器以控制电机速度。
SetMotorDirection((desiredSpeed >= 0.0) ? true : false); //使用SetMotorDirection()函数设置电机的方向
}从寄存器中读取电机编码器的值
//获取编码器计数值
signed short Motor::GetMotorCount(void) {
signed short motorCount; //带符号的16位整数
motorCount = IORD(motor_MeasureAddress, motor_measure_count_read_reg); //从内存映射寄存器读取速度计数
motorCount = motorCount - 0x8000; //减去0x8000清除count最高位（即最高位为1的标志位）以获取实际计数值
return motorCount;
}3.2 主程序
在主程序中实现PID算法
float kp = 0.02; //比例增益
float ki = 0.015; //积分增益
float kd = 0.35; //微分增益
float calculatePID(float error, float integral, float prev_error) {
float p = kp * error; //偏差error = 目标值 - 当前值
float i = ki * integral; //误差和
float d = kd * (error - prev_error);
return p + i + d;
}在主函数中实现对电机的控制，使其能够渐近设定的编码器目标值
int main()
{
int targetDistance; // 将此值更改为所需的目标距离
scanf("%d", &targetDistance);
printf("Hello BAL-Car,I'll keep moving\r\n");
Motor.StopMotor();
Motor.StartMotor();
float initialSpeed = 0; // 初始速度
Motor.SetSpeed(initialSpeed);
// 初始化PID变量
float integral = 0;
float prev_error = 0;
bool reach = false;
while(!reach){
// 测量当前编码器计数
int currentCounts = Motor.GetMotorCount();
printf("编码器：%d\n", currentCounts);
// 计算误差，即目标距离与当前距离的差值
int error = targetDistance - currentCounts;
// 计算 PID 控制输出
float controlOutput = calculatePID(error, integral, prev_error);
// 将控制输出限制在电机速度范围内
float speed = initialSpeed + controlOutput;
speed = fmaxf(-100, fminf(speed, 100));
// 更新下次迭代的前一次误差和积分
prev_error = error;
integral += error;
if(integral>10000) integral= 10000; //避免累积误差过大
if(integral<-10000) integral=-10000;
Motor.SetMotorSpeed(speed);
// 打印当前误差和当前速度
printf("误差：%d，当前速度：%.2f\n", error, speed);
usleep(1000*100);
if(error==0) {
printf("Enter next targetDistance");
scanf("%d", &targetDistance);
}
//reach = true;
}
return 0;
}四、实验结果
电机首先快速向目标编码值转动，最后逐渐收敛，但PID太难调了，调了好久还是会出现震荡
