CPLD设计PID控制器

ID:682837 · 发表于 2021-5-27 10:17

使用Verilog语言，编译成功

`timescale 1ns/100ps//时间尺度
module pidctrl3 (
isetpoint, // desire quater period 期望四分之一周期
ifeedback, // quater period measured 四分之一周期测量
iupdate,
iclk,
irst,
ikp,
iki,
ikd,
isynvalue,
iscale,
ilimit,
ierrlimit,
oreferr,
oresult, // oresultput flux/strength溢出/强度
ordy
);
input [15:0] isetpoint;
input [15:0] ifeedback;
input iupdate;
input iclk;
input irst;
input [15:0] ikp;
input [15:0] iki;
input [15:0] ikd;
input [15:0] isynvalue;
input [11:0] iscale;
input [15:0] ilimit;
input [15:0] ierrlimit;
output [15:0] oreferr;
output [15:0] oresult;
output ordy;
reg [15:0] mula;
reg [15:0] mulb;
wire [30:0] mulc;
reg [30:0] mulcq;
reg [38:0] accq;
wire [39:0] acc;
reg [27:0] integralq;
reg ordy;
reg [7:0] statq;
reg [15:0] errq;
wire [16:0] tmperr;
wire [16:0] tmperrabs;
reg [15:0] derivq;
wire chklimit;
reg [45:0] delta;
reg [45:0] deltaq;
reg [47:0] deltasumq;
wire [38:0] satdeltasum;
wire [15:0] saterr;
reg [15:0] saterrq;
wire [15:0] effectiveerr;
reg [38:0] satacc;
reg [3:0] selscale;
wire [15:0] fixresult;
wire [15:0] absresult;
reg [15:0] oresult;
wire oversat;
assign tmperr = {isetpoint[15],isetpoint} - {ifeedback[15],ifeedback};
assign tmperrabs = tmperr[16] ? ~tmperr : tmperr;
assign oversat = tmperrabs > {ierrlimit[15],ierrlimit};
assign saterr = tmperr[16] ? (oversat ? ~ierrlimit : tmperr[15:0]) :
(oversat ? ierrlimit : tmperr[15:0]);
// always @(*)
// case (err[16:15])
// 2'b01 : saterr = 16'h7fff;
// 2'b10 : saterr = 16'h8001;
// default : saterr = err[15:0];
// endcase
always @(posedge iclk)
if (irst) saterrq <= 'h0;
else saterrq <= iupdate ? saterr : saterrq;
//##################################################################
always @(posedge iclk)
if (irst) begin
errq <= 'h0; // it records errors between feedbacks and targets
derivq <= 'h0; // it records the changes of errq
end else begin
errq <= statq[0] ? saterrq : errq;
derivq <= statq[0] ? saterrq - errq : derivq;
end
//##################################################################
always @(*) // To save the number mulipliers, one single multiplier is time-multiplexed
case (statq[3:1])
3'b001 : begin mula = errq; mulb = ikp; end // perform PID, proportional calculation
3'b010 : begin mula = integralq[27:12]; mulb = iki; end // perform PID, integral calculation
3'b100 : begin mula = derivq; mulb = ikd; end // perform PID, derivative calculation
default : begin mula = 'h0; mulb = 'h0; end
endcase
assign mulc = {{15{mula[15]}},mula} * {{15{mulb[15]}},mulb};
always @(posedge iclk)
if (irst) integralq <= 'h0; // it records the sum of recent errq
else integralq <= statq[1] ? integralq + {{12{errq[15]}},errq} - {{12{integralq[27]}},integralq[27:12]} : integralq;
// ##################################################
always @(*)
case (statq[4:2])
3'b010 : selscale = iscale[7:4];
3'b100 : selscale = iscale[3:0];
default : selscale = iscale[11:8];
endcase
always @(*)
case (selscale) // A scaler that enlarge PID gains
4'h8 : delta = {{15{mulcq[30]}} ,mulcq};
4'h9 : delta = {{14{mulcq[30]}} ,mulcq, 1'h0};
4'ha : delta = {{13{mulcq[30]}} ,mulcq, 2'h0};
4'hb : delta = {{12{mulcq[30]}} ,mulcq, 3'h0};
4'hc : delta = {{11{mulcq[30]}} ,mulcq, 4'h0};
4'hd : delta = {{10{mulcq[30]}} ,mulcq, 5'h0};
4'he : delta = {{9{mulcq[30]}} ,mulcq, 6'h0};
4'hf : delta = {{8{mulcq[30]}} ,mulcq, 7'h0};
4'h1 : delta = {{6{mulcq[30]}} ,mulcq, 9'h0};
4'h2 : delta = {{5{mulcq[30]}} ,mulcq, 10'h0};
4'h3 : delta = {{4{mulcq[30]}} ,mulcq, 11'h0};
4'h4 : delta = {{3{mulcq[30]}} ,mulcq, 12'h0};
4'h5 : delta = {{2{mulcq[30]}} ,mulcq, 13'h0};
4'h6 : delta = {{1{mulcq[30]}} ,mulcq, 14'h0};
4'h7 : delta = {mulcq, 15'h0};
default : delta = {{7{mulcq[30]}} ,mulcq, 8'h0};
endcase
always @(posedge iclk)
if (irst) deltaq <= 'h0;
else deltaq <= delta;
//##################################
always @(posedge iclk)
if (irst) deltasumq <= 'h0;
else if (statq[0]) deltasumq <= 'h0;
else if (|statq[5:3]) deltasumq <= deltasumq + {deltaq[45],deltaq[45],deltaq};
else deltasumq <= deltasumq;
//##################################
assign satdeltasum = deltasumq[47] ? (&deltasumq[47:38] ? deltasumq[38:0] : 39'h40_0000_0001)
: (|deltasumq[47:38] ? 39'h3f_ffff_ffff : deltasumq[38:0]);
assign acc = {accq[38],accq} + {satdeltasum[38],satdeltasum};
always @(*)
case (acc[39:38])
2'b10 : satacc = 39'h40_0000_0001;
2'b01 : satacc = 39'h3f_ffff_ffff;
default : satacc = acc[38:0];
endcase
always @(posedge iclk)
if (irst) begin
ordy <= 'h0;
accq <= {isynvalue,23'h0}; // It is the last output result
oresult <= isynvalue;
end else begin
ordy <= statq[7];
accq <= statq[7] ? {fixresult,accq[22:0]} : (statq[6] ? satacc : accq);
oresult <= statq[7] ? fixresult : oresult;
end
//##########################################
always @(posedge iclk)
if (irst) begin
statq <= 'h0; // used to record calculation status, it take 6 cycles to do 1 PID iteration
mulcq <= 'h0; // A flip-flop to stored result from multiplier
end else begin
statq <= {statq[6:0],iupdate};
mulcq <= mulc;
end
assign absresult = accq[38] ? ~accq[38:23] : accq[38:23];
assign chklimit = absresult > ilimit;
assign fixresult = chklimit ? (accq[38] ? ~ilimit : ilimit) : accq[38:23];
assign oreferr = errq;
endmodule
`ifdef USETESTBENCH
module tb_pidctrl3();
reg clk;
reg rst;
reg [15:0] setpoint;
wire [15:0] feedbackx;
wire [15:0] feedbacky;
reg [4:0] noise;
wire [15:0] resultx;
wire [15:0] resulty;
reg update;
initial begin
clk = 0;
forever clk = #5 ~clk;
end
initial begin
rst = 1;
setpoint = 0;
#1000;
rst = 0;
setpoint = 16'h100;
#100000;
setpoint = 16'h200;
#100000;
setpoint = 16'hfe00;
#100000;
setpoint = 16'hfd00;
#100000;
setpoint = 16'h400;
#100000;
setpoint = 16'h7ff;
#100000;
setpoint = 16'hf800;
#100000;
setpoint = 16'h100;
#100000;
setpoint = 16'hf000;
#100000;
setpoint = 16'h5000;
#100000;
end
initial begin
noise = 0;
update = 0;
forever begin
noise = $random();
update = 1;
#10;
update = 0;
#100;
end
end
assign feedbackx = { {5{resultx[15]}},resultx[15:5]} + {{11{noise[4]}},noise};
assign feedbacky = { {5{resulty[15]}},resulty[15:5]} + {{11{noise[4]}},noise};
pidctrl3 pidctrl3 (
.isetpoint(setpoint), // desire quater period
.ifeedback(feedbackx), // quater period measured
.iupdate(update),
.iclk(clk),
.irst(rst),
.ikp(16'h4000),
.iki(16'h3000),
.ikd(16'h2000),
.isynvalue(16'h0),
.iscale(12'heee),
.ilimit(16'h2000),
.ierrlimit(16'hffff),
.oreferr(),
.oresult(resultx), // oresultput flux/strength
.ordy()
);
pidctrl pidctrl (
.isetpoint(setpoint), // desire quater period
.ifeedback(feedbacky), // quater period measured
.iupdate(update),
.iclk(clk),
.irst(rst),
.ikp(16'h4000),
.iki(16'h3000),
.ikd(16'h2000),
.isynvalue(16'h0),
.ilimiter(16'h7fff),
.iscaler(4'he),
.oreferr(),
.oresult(resulty), // oresultput flux/strength
.ordy()
);
endmodule
`endif

复制代码

帐号		自动登录	找回密码
密码			立即注册

CPLD设计PID控制器

评分

相关帖子