1 Knowledge background
PID control should be regarded as a very widely used control algorithm. Common ones include controlling the ambient temperature, controlling the flight height and speed of the drone, etc. We divide PID into three parameters, as follows:
P-proportional control, the basic function is to increase the control object in a linear manner, and dynamically output at a constant ratio. The disadvantage is that it will produce a steady-state error.
I-Integral control, its basic function is to eliminate the steady-state error, but its disadvantage is that it will produce overshoot.
D-Differential control, its basic function is to weaken the overshoot phenomenon and increase the inertial response speed.
PID control system principle block diagram
PID formula
In general, when the output of the system is obtained, the output is added to the input through three operation methods: proportion, integral, and differential, thus forming a closed-loop control system. In real practice, the most difficult thing is how to determine the coefficients of the three terms, which requires a lot of experiments and experience to determine. Through constant trying and thinking, you can select the appropriate parameters and make an excellent PID controller.
2 System framework
No matter how much theory is said, it is better to practice it yourself. I made a simple PID control model. Because PID is used to control the PWM duty cycle, the output PWM duty cycle and the collected duty cycle are basically different. There are errors, so there are still some differences from real PID closed-loop control. This is just an example to illustrate how to use FPGA to make a simple PID algorithm. Then you can observe the curve changes of adjusting the duty cycle through Modelsim simulation. Finally, verify it on the development board. Use an oscilloscope to test the output PWM duty cycle and compare it with our settings. The target duty cycle is consistent. If we want to control the motor speed, there is friction resistance and speed acquisition error. At this time, we need to adjust Kp, Ki, and Kd to achieve the best control effect. The model block diagram is as follows:
target: target value
actual: actual value
3 Experimental requirements and purposes
Generates a PWM signal with a fixed frequency and a duty cycle that can be adjusted by pressing the button. When KEY1 is pressed, the duty cycle increases by 10%, and when KEY2 is pressed, the duty cycle decreases by 10%. A PID calculation is performed for each PWM cycle.
4 Required Hardware
- ALOGIC_V4 FPGA development board
- FPGA downloader
- Oscilloscope
5 Formula Analysis
We have listed the formulas of PID theory before, but it is still a bit confusing to implement it using Verilog language just by looking at the theoretical formulas, so we need to make slight changes in the company and convert it into a formula that is convenient for using FPGA to implement PID:
Kp: proportional term parameter
Ki: integral term parameter
Kd: differential term parameter
error: error, target-actual
sum_error: sum of errors
error-last_error: current error minus the previous error
6 Programming
The program block diagram is as follows:
key_xd: key debounce module, this module directly calls the debounce routine of our development board.
target_gen: Target value generation module, which generates the desired duty cycle value. Since the FPGA cannot handle decimals, the data is expanded 100 times to facilitate processing. If the set value is 980, the actual duty cycle is 9.8%.
module targe_gen(
input clk,
input rst_n,
input key_add,
input key_sub,
output reg rst_n_out,
output reg [15:0] targe
);
reg [7:0] rst_cnt;
always@(posedge clk or negedge rst_n)begin
if(!rst_n)begin
targe<=3500;
rst_n_out<=0;
end else if(key_add)begin
targe<=targe+1000;
rst_n_out<=0;
end else if(key_sub)begin
targe<=targe-1000;
rst_n_out<=0;
end else if(rst_cnt==100)begin
rst_n_out <=1;
end
end
always@(posedge clk or negedge rst_n)begin
if(!rst_n)
rst_cnt<=0;
else if(key_add||key_sub)
rst_cnt<=0;
else if(rst_n_out==0)
rst_cnt<=rst_cnt+1;
else
rst_cnt<=0;
end
endmodule
zkb_calc: Duty cycle detection module, calculates the actual output PWM duty cycle, that is, the target value. Since the FPGA cannot handle decimals, the data is expanded 100 times to facilitate processing. If the set value is 980, the actual duty cycle is 9.8%. This module requires division. We cannot use "/" directly in the code to perform calculations. Instead, we need to call the divider IPCORE to perform calculations.
module zkb_calc(
input clk ,
input rst_n ,
input pwm_in ,//反馈信号
output reg [15:0] pwm_zkb ,//计算的PWM占空比
output reg pwm_zkb_vld //PWM占空比有效标志
);
parameter ST0 =4'd0;
parameter ST1 =4'd1;
parameter CALC_ST =4'd2;
parameter RESULT_ST =4'd3;
parameter time_out_num=500;//采样时间
reg [3:0] curr_st;
reg [31:0] pwm_hcnt;
reg [31:0] pwm_hlcnt;
reg [31:0] div_dividend;
reg [31:0] div_divisor;
reg div_ce;
reg [31:0] time_out_cnt;
reg pwm_in_ff1,pwm_in_ff2,pwm_in_ff3;
wire[39:0] quotient;
reg rdy;
assign pwm_in_rise=pwm_in_ff2&&(pwm_in_ff3==0);
always@(posedge clk)pwm_in_ff1<=pwm_in;
always@(posedge clk)pwm_in_ff2<=pwm_in_ff1;
always@(posedge clk)pwm_in_ff3<=pwm_in_ff2;
always@(posedge clk)rdy<=div_ce;
always@(posedge clk or negedge rst_n)begin
if(!rst_n)begin
pwm_zkb <=0;
pwm_zkb_vld <=0;
end else if(rdy)begin
pwm_zkb <=quotient[15:0];
pwm_zkb_vld <=1;
end else
pwm_zkb_vld<=0;
end
always@(posedge clk or negedge rst_n)begin
if(!rst_n)begin
curr_st <=ST0;
div_ce <=0;
div_dividend<=0;
div_divisor<=0;
end else case(curr_st)
ST0:begin
if(time_out_cnt==1)
curr_st<=ST1;
else;
end
ST1:begin
if(time_out_cnt==1)
curr_st<=CALC_ST;
else;
end
CALC_ST:begin
curr_st<=RESULT_ST;
div_dividend<={pwm_hcnt,13'h0}+{pwm_hcnt,10'h0}+{pwm_hcnt,9'h0}+{pwm_hcnt,8'h0}+{pwm_hcnt,4'h0};//x10000
div_divisor<=pwm_hlcnt;
div_ce<=1;
end
RESULT_ST:begin
div_ce<=0;
curr_st<=ST0;
end
default:;
endcase
end
always@(posedge clk or negedge rst_n)begin
if(!rst_n)
time_out_cnt<=0;
else if(time_out_cnt==time_out_num-1)
time_out_cnt<=0;
else
time_out_cnt<=time_out_cnt+1;
end
always@(posedge clk or negedge rst_n)begin
if(!rst_n)
pwm_hcnt<=0;
else if(curr_st==RESULT_ST)
pwm_hcnt<=0;
else if(curr_st==ST1&&pwm_in_ff3)
pwm_hcnt<=pwm_hcnt+1;
else;
end
always@(posedge clk or negedge rst_n)begin
if(!rst_n)
pwm_hlcnt<=0;
else if(curr_st==ST0)
pwm_hlcnt<=0;
else if(curr_st==ST1)
pwm_hlcnt<=pwm_hlcnt+1;
else;
end
DIV U_DIV(
.denom ({8'b0,div_divisor}),//被除数
.numer ({8'b0,div_dividend}),//除数
.quotient(quotient),
.remain ()
);
endmodule
pid_ctrl: pid calculation module, according to the above formula, addition and multiplication are needed. For addition, we can use "+" directly in the code to calculate. For multiplication, we need to call the IPCORE of the multiplier (because the error is divided into positive and negative , so our multiplier IPCORE also needs to be set to signed, this must be paid attention to, otherwise there will be problems in the calculation), and finally the duty cycle value is calculated. Since the FPGA cannot handle decimals, Kp, Ki, Kd are expanded 100 times. If Kp=10, the real value is 0.1. According to the formula, we know that the final PWM duty cycle value is expanded by 10,000 times.
module pid_ctrl(
input clk ,
input rst_n ,
input [15:0] targe ,//x100
input [15:0] actual ,//x100
input actual_vld ,
output reg [31:0] pwm_zkb , //x10000，因为targe,actual乘以100，Kp,Ki,Kd乘以100，所以结果放大了10000
output reg pwm_zkb_vld
);
parameter IDLE =8'd0;
parameter STEP1 =8'd1;
parameter STEP2 =8'd2;
parameter STEP3 =8'd3;
parameter STEP4 =8'd4;
parameter STEP5 =8'd5;
parameter STEP6 =8'd6;
parameter Kp =10;//x100;
parameter Ki =10;//x100;
parameter Kd =15;//x100;
reg [7:0] curr_st;
reg [31:0] sum_error;
reg [31:0] last_error;
reg [31:0] error ;
reg [31:0] mul1_a,mul2_a,mul3_a;
reg [31:0] mul1_b,mul2_b,mul3_b;
reg mul1_ce,mul2_ce,mul3_ce;
wire[31:0] mul1_result,mul2_result,mul3_result;
always@(posedge clk or negedge rst_n)begin
if(!rst_n)begin
curr_st <=IDLE;
sum_error <=0;
last_error <=0;
error <=0;
pwm_zkb <=0;
mul1_a <=0;
mul1_b <=0;
mul1_ce <=0;
mul2_a <=0;
mul2_b <=0;
mul2_ce <=0;
mul3_a <=0;
mul3_b <=0;
mul3_ce <=0;
pwm_zkb_vld <=0;
end else case(curr_st)
IDLE:begin
pwm_zkb_vld<=0;
if(actual_vld)begin
curr_st<=STEP1;
end else;
end
STEP1:begin
last_error<=error;
curr_st<=STEP2;
end
STEP2:begin
error<=targe-actual;
curr_st<=STEP3;
end
STEP3:begin
sum_error<=sum_error+error;
curr_st<=STEP4;
end
STEP4:begin
mul1_a<=Kp;
mul1_b<=error;
mul1_ce<=1;
mul2_a<=Ki;
mul2_b<=sum_error;
mul2_ce<=1;
mul3_a<=Kd;
mul3_b<=error-last_error;
mul3_ce<=1;
curr_st<=STEP5;
end
STEP5:curr_st<=STEP6;
STEP6:begin
mul1_ce<=0;
mul2_ce<=0;
mul3_ce<=0;
pwm_zkb<=mul1_result+mul2_result+mul3_result;
pwm_zkb_vld<=1;
curr_st<=IDLE;
end
default;
endcase
end
MUL_SIGN_32X32 U_MUL1(
.clock (clk ), // input clk
.aclr (~rst_n ),
.dataa (mul1_a ), // input [15 : 0] a
.datab (mul1_b ), // input [15 : 0] b
.clken (mul1_ce ), // input ce
.result (mul1_result) // output [31 : 0] p
);
MUL_SIGN_32X32 U_MUL2(
.clock (clk ), // input clk
.aclr (~rst_n ),
.dataa (mul2_a ), // input [15 : 0] a
.datab (mul2_b ), // input [15 : 0] b
.clken (mul2_ce ), // input ce
.result (mul2_result) // output [31 : 0] p
);
MUL_SIGN_32X32 U_MUL3(
.clock (clk ), // input clk
.aclr (~rst_n ),
.dataa (mul3_a ), // input [15 : 0] a
.datab (mul3_b ), // input [15 : 0] b
.clken (mul3_ce ), // input ce
.result (mul3_result) // output [31 : 0] p
);
endmodule
pwm_drv: According to the duty cycle calculated by the pid_ctrl module, the PWM signal is output. The PWM signal is divided into two. One channel is output through the FPGA pin. The waveform can be observed with an oscilloscope. The other channel is directly passed to the zkb_calc module, thus forming a Closed loop system. This module needs to use multiplication and division calculations and needs to call the corresponding IPCORE.
module pwm_drv(
input clk,
input rst_n,
input [31:0] pwm_zkb,//x10000
input pwm_zkb_vld,
output reg pwm
);
parameter period_num=500;//FREQ 10K，频率越大，可调占空比精度越低，实测如果频率是100K，占空比只能精确到个位，频率是10K，可确到小数点后1位。
reg [19:0] period_cnt;
reg [31:0] hcnt;
reg [35:0] div_dividend;
reg [31:0] div_divisor;
reg div_ce;
wire[31:0] quotient;
reg rdy;
reg [31:0] mul_a,mul_b;
reg mul_ce;
wire[63:0] mul_result;
reg [3:0] curr_st;
always@(posedge clk)rdy<=div_ce;
always@(posedge clk or negedge rst_n)begin
if(!rst_n)
hcnt<=0;
else if(rdy)
hcnt<=quotient[31:0];
else;
end
always@(posedge clk or negedge rst_n)begin
if(!rst_n)
period_cnt<=0;
else if(period_cnt==period_num-1)
period_cnt<=0;
else
period_cnt<=period_cnt+1;
end
always@(posedge clk or negedge rst_n)begin
if(!rst_n)begin
curr_st<=0;
mul_a<=0;
mul_b<=0;
mul_ce<=0;
div_ce<=0;
div_dividend<=0;
div_divisor<=0;
end else case(curr_st)
0:begin
div_ce<=0;
if(pwm_zkb_vld)
curr_st<=1;
else
;
end
1:begin
mul_a<=period_num;
mul_b<=pwm_zkb;
mul_ce<=1;
curr_st<=2;
end
2:begin
mul_ce<=0;
curr_st<=3;
end
3:begin
div_dividend<=mul_result[63:0];
div_divisor<=1000000;//占空比扩大100倍，360就是3.6%，0.036，此处引起误差
div_ce<=1;
curr_st<=0;
end
default:;
endcase
end
always@(posedge clk or negedge rst_n)begin
if(!rst_n)
pwm<=0;
else if(period_cnt<hcnt[15:0])
pwm<=1;
else
pwm<=0;
end
MUL_UNSIGN_32X32 U_MUL(
.clock (clk ), // input clk
.aclr (~rst_n ),
.dataa (mul_a ), // input [15 : 0] a
.datab (mul_b ), // input [15 : 0] b
.clken (mul_ce ), // input ce
.result (mul_result) // output [31 : 0] p
);
DIV U_DIV(
.denom ({8'b0,div_divisor}),
.numer ({4'b0,div_dividend[35:0]}),
.quotient(quotient),
.remain ()
);
endmodule
7 Simulation (observe the adjustment effect through Modelsim simulation)
The duty cycle target value we set is 980, which is 9.8%.
7.1 The first set of PID parameters
Kp=0.1, Ki=0.03, Kd=0;
settings are as shown below:
The simulation is as shown below:
It can be seen through simulation that the duty cycle value (pwm_zkb) output by the pid_ctrl module is a smooth curve, slowly rising from 0 to 98160, and then stabilizing. The experimentally measured duty cycle (actual) is equal to 980, which is equal to the target value. During this process, the error also slowly decreases until the error is equal to 0. This is a closed-loop adjustment process.
7.2 The second set of PID parameters
Kp=0.1, Ki=0.15, Kd=0;
parameter settings are as follows:
The simulation is shown in the figure below:
We increased the Ki parameter and found that the rising slope of the signal became steeper. This has the advantage of shortening the adjustment time, so that the signal can reach our target value faster, but the signal has oscillation (overshoot) phenomenon, that is, It first exceeded the target value (980), and then slowly stabilized. The larger the Kp parameter, the more serious the overshoot phenomenon is. In actual use, we do not allow serious oscillation phenomena to occur, because this will cause problems in our control system. For example, when we control a drone, if we set a height of 1,000 meters, if the oscillation is severe, the drone will suddenly rise to a height of more than 1,000 meters, and then drop to 1,000 meters. During the oscillation process, if there is a drone at 1,200 meters Obstacle, then the drone will hit the obstacle, causing serious consequences, so we must avoid serious overshoot. To solve the overshoot phenomenon, two parameters, Ki and Kd, need to be adjusted.
7.3 The third set of PID parameters
Kp=0.1, Ki=0.1, Kd=0.15;
set as shown below:
The simulation is shown in the figure below:
It can be clearly observed that the overshoot phenomenon is reduced a lot.
8 Board verification
Use our ALOGIC_V4 development board to verify that the duty cycle can reach our target value. As shown below:
