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 卡尔曼滤波

卡尔曼滤波主要是为了对霍尔传感器输出的方波进行滤波操作

cpp 复制代码
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

endmodule

3.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去控制电机的转速

cpp 复制代码
`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

endmodule

3.1.2 电机监测

电机转动带动磁盘经过霍尔传感器,磁力的变化让霍尔效应传感器产生霍尔效应电压,经过数字电路处理产生方波,两个位置不同的霍尔效应传感器输出两个相位不同的方波(PhaseA和Phase B)。磁盘在转动时,先被感应的传感器会先输出方波,另一个传感器输出会有延迟,所以两个方波的相位有所不同。由此可以通过方波相位领先计算电机的方向。根据输出的脉冲数,可以计算电机转速。

首先计算电机的方向

cpp 复制代码
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

检测编码器的值

cpp 复制代码
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组成,对相关函数进行声明和定义

首先在构造函数中传入电机的地址和测量寄存器地址

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);
}

控制电机的启动、停止以及方向

cpp 复制代码
//启动电机
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寄存器中
}

设置电机转动的速度

cpp 复制代码
//设置电机速度
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()函数设置电机的方向
}

从寄存器中读取电机编码器的值

cpp 复制代码
//获取编码器计数值
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算法

cpp 复制代码
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;
}

在主函数中实现对电机的控制,使其能够渐近设定的编码器目标值

cpp 复制代码
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太难调了,调了好久还是会出现震荡

相关推荐
m0_739312872 小时前
【STM32】项目实战——OV7725/OV2604摄像头颜色识别检测(开源)
stm32·单片机·嵌入式硬件
嵌入式小章2 小时前
基于STM32的实时时钟(RTC)教学
stm32·嵌入式硬件·实时音视频
TeYiToKu3 小时前
笔记整理—linux驱动开发部分(9)framebuffer驱动框架
linux·c语言·arm开发·驱动开发·笔记·嵌入式硬件·arm
基极向上的三极管4 小时前
【AD】3-4 在原理图中放置元件
嵌入式硬件
徐嵌4 小时前
STM32项目---水质水位检测
stm32·单片机·嵌入式硬件
lantiandianzi4 小时前
基于单片机的老人生活安全监测系统
单片机·嵌入式硬件·生活
东胜物联4 小时前
探寻5G工业网关市场,5G工业网关品牌解析
人工智能·嵌入式硬件·5g
stm32发烧友4 小时前
基于STM32的智能家居环境监测系统设计
stm32·嵌入式硬件·智能家居
上理考研周导师4 小时前
第二章 虚拟仪器及其构成原理
fpga开发
FPGA技术实战6 小时前
《探索Zynq MPSoC》学习笔记(二)
fpga开发·mpsoc