FPGA实现电机位置环、速度环双闭环PID控制

一、设计思路

主要设计思路就是根据之前写的一篇FPGA实现电机转速PID控制，前面已经实现了位置环的控制，思想就是通过电机编码器的当前位置值不断地修正PID去控制速度。

那为了更好的实现控制，可以在位置环后加上速度环，实现电机位置环、速度环双闭环PID控制。

位置环作为外环，通过编码器计数通过PID输出速度；位置环输出的速度作为目标速度输入速度环，与编码器测速的当前速度进行PID计算，从而完成电机的双PID控制。

二、位置环控制

位置环的控制在前面已经实现，再次不再赘述。

三、速度环控制

速度环作为内环，目标值为位置环输出的速度，当前值为编码器测速的速度。

这里就有了一个问题，编码器怎么样才能测到电机速度？

这就和电机的最大频率有关，编码器通过AB相位的波形去进行计数和正反转的判断，那么就需要知道电机转一圈所输出的脉冲个数以及空载最大转速，通过这两个参数计算出编码器的电机最大频率，即每秒会有多少个脉冲。因此要对时钟进行分频，这是对50MHZ时钟分频成100HZ，即时钟周期10ms进行一次PID输出。

always @(posedge clk or negedge reset_n) beginif (~reset_n) beginclk100 <= 1'b0;clk_counter <= 32'd0;end else if (clk_counter == 32'd249999)begin//分频系数M=时钟输入频率/时钟输出频率，偶分频计数器值N=M/2；奇分频计数器值N=M-1/2clk100 <= ~clk100;clk_counter <= 32'd0;end else beginclk_counter <= clk_counter + 1;end
end

速度环的PID计算同位置环一样

always @(posedge clk100 or negedge reset_n) beginif (~reset_n) beginerror      <= 0;integral   <= 0;prev_error <= 0;derivative <= 0;pidoutput  <= 0;end else begin//target位置环输出的速度，current当前编码器值error <= target - current;// 将累积误差控制合理范围内integral <= integral + error;if(integral >= $signed(32'd500)) beginintegral <= $signed(32'd500);end else if(integral <= $signed(-32'd500M)) beginintegral <= $signed(-32'd500);end else beginintegral <= integral;endderivative <= error - prev_error;// 计算PID输出p <= error * kp;i <= integral * ki;d <= derivative * kd;pidoutput <= (p + i + d) / 1000;end
end

四、编码器测速

编码器模块也需要加入测速代码，有两种测速方法：一种是两次位置值相减，得出电机速度；另一种是编码器计数，得出电机速度。

这里用第二种编码器计数，在速度环中每10ms输出一次PID值，那么在测速模块就每10ms将速度编码器置零重新计数，因此可以根据速度环传入的清零信号clear进行清零，即clk100。当clk100状态发生变化时，clear信号为1，清零速度计数。

always@(posedge clk or negedge reset_n) beginif(!reset_n) begindelay0 <= 1'b0;delay1 <= 1'b0;		end else begindelay0 <= clk100;delay1 <= delay0;end
end
assign clear  = (delay0 & (~delay1));always @(posedge clk or negedge reset_n) beginif (~reset_n) begincounter <= 0;endelse if (clear) begincounter <= 0;end else if(pulse) beginif(direction) beginif(counter < $signed(32'h8000))counter <= counter + 1;end else if(!direction) beginif(counter > $signed(-32'h8000))counter <= counter - 1;end else begincounter <= 0;     end   end
end