| Line 1... |
Line 1... |
/* PID controller
|
/* PID controller
|
Author: Zhu Xu
|
|
Email: m99a1@yahoo.cn
|
|
|
|
sigma=Ki*e(n)+sigma
|
sigma=Ki*e(n)+sigma
|
u(n)=(Kp+Kd)*e(n)+sigma+Kd*(-e(n-1))
|
u(n)=(Kp+Kd)*e(n)+sigma+Kd*(-e(n-1))
|
|
|
Data width of Wishbone slave port can be can be toggled between 64-bit, 32-bit and 16-bit.
|
Data width of Wishbone slave port can be can be toggled between 64-bit, 32-bit and 16-bit.
|
| Line 10... |
Line 8... |
|
|
Wishbone compliant
|
Wishbone compliant
|
Work as Wishbone slave, support Classic standard SINGLE/BLOCK READ/WRITE Cycle
|
Work as Wishbone slave, support Classic standard SINGLE/BLOCK READ/WRITE Cycle
|
|
|
registers or wires
|
registers or wires
|
[15:0]kp,ki,kd,sp,pv; can be both read and written through Wishbone interface, address: 0x0, 0x4, 0x8, 0xc, 0x10
|
[15:0]kp,ki,kd,sp,pv; can be both read and written through Wishbone interface, address: 0x0, 0x4, 0x8, 0x12, 0x16
|
[15:0]kpd; read only through Wishbone interface, address: 0x14
|
[15:0]kpd; read only through Wishbone interface, address: 0x20
|
[15:0]err[0:1]; read only through Wishbone interface, address: 0x18, 0x1c
|
[15:0]err[0:1]; read only through Wishbone interface, address: 0x24, 0x28
|
[15:0]mr,md; not accessable through Wishbone interface
|
[15:0]mr,md; not accessable through Wishbone interface
|
[31:0]p,b; not accessable through Wishbone interface
|
[31:0]p,b; not accessable through Wishbone interface
|
[31:0]un,sigma; read only through Wishbone interface, address: 0x20, 0x24
|
[31:0]un,sigma; read only through Wishbone interface, address: 0x32, 0x36
|
|
|
|
|
[4:0]OF; overflow register, read only through Wishbone interface, address: 0x28
|
[4:0]of; overflow register, read only through Wishbone interface, address: 0x40
|
OF[0]==1 : kpd overflow
|
of[0]==1 : kpd overflow
|
OF[1]==1 : err[0] overflow
|
of[1]==1 : err[0] overflow
|
OF[2]==1 : err[1] overflow
|
of[2]==1 : err[1] overflow
|
OF[3]==1 : un overflow
|
of[3]==1 : un overflow
|
OF[4]==1 : sigma overflow
|
of[4]==1 : sigma overflow
|
[0:15]rl; read lock, when asserted corelated reagister can not be read through Wishbone interface
|
[0:15]rl; read lock, when asserted corelated reagister can not be read through Wishbone interface
|
[0:7]wl; write lock, when asserted corelated reagister can not be written through Wishbone interface
|
[0:7]wl; write lock, when asserted corelated reagister can not be written through Wishbone interface
|
|
|
|
|
|
|
| Line 45... |
Line 43... |
`ifdef wb_64bit
|
`ifdef wb_64bit
|
parameter wb_nb=64,
|
parameter wb_nb=64,
|
`endif
|
`endif
|
adr_wb_nb=16
|
adr_wb_nb=16
|
)(
|
)(
|
//Wishbone Slave Interface
|
|
input i_clk,
|
input i_clk,
|
input i_rst,
|
input i_rst, //reset when low
|
|
//Wishbone Slave port
|
input i_wb_cyc,
|
input i_wb_cyc,
|
input i_wb_stb,
|
input i_wb_stb,
|
input i_wb_we,
|
input i_wb_we,
|
input [adr_wb_nb-1:0]i_wb_adr,
|
input [adr_wb_nb-1:0]i_wb_adr,
|
input [wb_nb-1:0]i_wb_data,
|
input [wb_nb-1:0]i_wb_data,
|
| Line 71... |
Line 69... |
kpd_adr = 5,
|
kpd_adr = 5,
|
err_0_adr = 6,
|
err_0_adr = 6,
|
err_1_adr = 7,
|
err_1_adr = 7,
|
un_adr = 8,
|
un_adr = 8,
|
sigma_adr = 9,
|
sigma_adr = 9,
|
OF_adr = 10;
|
of_adr = 10;
|
|
|
wire rst;
|
|
assign rst=~i_rst;
|
|
|
|
reg [15:0]kp,ki,kd,sp,pv;
|
reg [15:0]kp,ki,kd,sp,pv;
|
reg wlkp,wlki,wlkd,wlsp,wlpv;
|
reg wlkp,wlki,wlkd,wlsp,wlpv; // write lock, if one is high the relevant reg is not writeable
|
|
|
wire [0:7]wl={wlkp,wlki,wlkd,wlsp,wlpv,3'h0};
|
wire [0:7]wl={wlkp,wlki,wlkd,wlsp,wlpv,3'h0}; // write lock
|
|
|
reg wack; //write acknowledged
|
reg wack; //write acknowledged
|
|
|
wire [2:0]adr;
|
wire [2:0]adr;
|
`ifdef wb_16bit
|
`ifdef wb_16bit
|
| Line 136... |
Line 131... |
`ifdef wb_64bit
|
`ifdef wb_64bit
|
assign adr_check=i_wb_adr[6]==0&&adr_check_1;
|
assign adr_check=i_wb_adr[6]==0&&adr_check_1;
|
`endif
|
`endif
|
|
|
//state machine No.1
|
//state machine No.1
|
always@(posedge i_clk or negedge rst)
|
always@(posedge i_clk or posedge i_rst)
|
if(!rst)begin
|
if(i_rst)begin
|
state_0<=0;
|
state_0<=0;
|
wack<=0;
|
wack<=0;
|
kp<=0;
|
kp<=0;
|
ki<=0;
|
ki<=0;
|
kd<=0;
|
kd<=0;
|
| Line 188... |
Line 183... |
endcase
|
endcase
|
end
|
end
|
|
|
|
|
//state machine No.2
|
//state machine No.2
|
reg [14:0]state_2;
|
reg [11:0]state_1;
|
reg state_1;
|
|
|
|
wire update_kpd;
|
wire update_kpd;
|
assign update_kpd=wack&&(~adr[2])&&(~adr[0])&&adr_check; //adr==0||adr==2
|
assign update_kpd=wack&&(~adr[2])&&(~adr[0])&&adr_check; //adr==0||adr==2
|
|
|
wire update_esu; //update e(n), sigma and u(n)
|
wire update_esu; //update e(n), sigma and u(n)
|
assign update_esu=wack&&(adr==4)&&adr_check;
|
assign update_esu=wack&&(adr==4)&&adr_check;
|
|
|
reg rlkpd;
|
reg rlkpd; // read lock
|
reg rlerr_0;
|
reg rlerr_0; // read lock
|
reg rlerr_1;
|
reg rlerr_1; // read lock
|
reg rla;
|
reg rla; // read lock
|
reg rlsigma;
|
reg rlsigma; // read lock
|
reg rlOF;
|
reg rlof; // read lock
|
|
|
reg [4:0]OF;
|
reg [4:0]of;
|
reg [15:0]kpd;
|
reg [15:0]kpd;
|
reg [15:0]err[0:1];
|
reg [15:0]err[0:1];
|
|
|
wire [15:0]mr,md;
|
wire [15:0]mr,md;
|
|
|
| Line 226... |
Line 220... |
reg cout;
|
reg cout;
|
wire cin;
|
wire cin;
|
wire [31:0]sum;
|
wire [31:0]sum;
|
wire [31:0]product;
|
wire [31:0]product;
|
|
|
wire OF_addition[0:1];
|
wire of_addition[0:1];
|
assign OF_addition[0]=(p[15]&&a[15]&&(!sum[15]))||((!p[15])&&(!a[15])&&sum[15]);
|
assign of_addition[0]=(p[15]&&a[15]&&(!sum[15]))||((!p[15])&&(!a[15])&&sum[15]);
|
assign OF_addition[1]=(p[31]&&a[31]&&(!sum[31]))||((!p[31])&&(!a[31])&&sum[31]);
|
assign of_addition[1]=(p[31]&&a[31]&&(!sum[31]))||((!p[31])&&(!a[31])&&sum[31]);
|
|
|
|
always@(posedge i_clk or posedge i_rst)
|
|
if(i_rst)begin
|
reg [31:0]reg_sum;
|
state_1<=12'b000000000001;
|
reg [31:0]reg_product;
|
|
reg reg_OF_addition[0:1];
|
|
|
|
always@(posedge i_clk)begin
|
|
reg_sum<=sum;
|
|
reg_OF_addition[0]<=OF_addition[0];
|
|
reg_OF_addition[1]<=OF_addition[1];
|
|
reg_product<=product;
|
|
end
|
|
|
|
always@(posedge i_clk or negedge rst)
|
|
if(!rst)begin
|
|
state_1<=0;
|
|
state_2<=15'b000000000000001;
|
|
wlkp<=0;
|
wlkp<=0;
|
wlki<=0;
|
wlki<=0;
|
wlkd<=0;
|
wlkd<=0;
|
wlsp<=0;
|
wlsp<=0;
|
wlpv<=0;
|
wlpv<=0;
|
rlkpd<=0;
|
rlkpd<=0;
|
rlerr_0<=0;
|
rlerr_0<=0;
|
rlerr_1<=0;
|
rlerr_1<=0;
|
rla<=0;
|
rla<=0;
|
rlsigma<=0;
|
rlsigma<=0;
|
rlOF<=0;
|
rlof<=0;
|
OF<=0;
|
of<=0;
|
kpd<=0;
|
kpd<=0;
|
err[0]<=0;
|
err[0]<=0;
|
err[1]<=0;
|
err[1]<=0;
|
p<=0;
|
p<=0;
|
a<=0;
|
a<=0;
|
| Line 273... |
Line 253... |
md_index<=0;
|
md_index<=0;
|
cout<=0;
|
cout<=0;
|
end
|
end
|
else begin
|
else begin
|
case(state_1)
|
case(state_1)
|
1: state_1<=0;
|
12'b000000000001: begin
|
0:begin
|
|
case(state_2)
|
|
15'b0000000000001: begin
|
|
if(update_kpd)begin
|
if(update_kpd)begin
|
state_2<=15'b000000000000010;
|
state_1<=12'b000000000010;
|
wlkp<=1;
|
wlkp<=1;
|
wlkd<=1;
|
wlkd<=1;
|
wlpv<=1;
|
wlpv<=1;
|
rlkpd<=1;
|
rlkpd<=1;
|
rlOF<=1;
|
rlof<=1;
|
end
|
end
|
else if(update_esu)begin
|
else if(update_esu)begin
|
state_2<=15'b00000000001000;
|
state_1<=12'b000000001000;
|
wlkp<=1;
|
wlkp<=1;
|
wlki<=1;
|
wlki<=1;
|
wlkd<=1;
|
wlkd<=1;
|
wlsp<=1;
|
wlsp<=1;
|
wlpv<=1;
|
wlpv<=1;
|
rlkpd<=1;
|
rlkpd<=1;
|
rlerr_0<=1;
|
rlerr_0<=1;
|
rlerr_1<=1;
|
rlerr_1<=1;
|
rla<=1;
|
rla<=1;
|
rlsigma<=1;
|
rlsigma<=1;
|
rlOF<=1;
|
rlof<=1;
|
end
|
end
|
end
|
end
|
15'b000000000000010: begin
|
12'b000000000010: begin
|
p<={{16{kp[15]}},kp};
|
p<={{16{kp[15]}},kp};
|
a<={{16{kd[15]}},kd};
|
a<={{16{kd[15]}},kd};
|
state_2<=15'b000000000000100;
|
state_1<=12'b000000000100;
|
state_1<=1;
|
|
end
|
end
|
15'b000000000000100: begin
|
12'b000000000100: begin
|
kpd<=reg_sum[15:0];
|
kpd<=sum[15:0];
|
wlkp<=0;
|
wlkp<=0;
|
wlkd<=0;
|
wlkd<=0;
|
wlpv<=0;
|
wlpv<=0;
|
rlkpd<=0;
|
rlkpd<=0;
|
rlOF<=0;
|
rlof<=0;
|
OF[0]<=reg_OF_addition[0];
|
of[0]<=of_addition[0];
|
state_2<=15'b000000000000001;
|
state_1<=12'b000000000001;
|
end
|
end
|
15'b000000000001000: begin
|
12'b000000001000: begin
|
p<={{16{~err[0][15]}},~err[0]};
|
p<={{16{~err[0][15]}},~err[0]};
|
a<={31'b0,1'b1};
|
a<={31'b0,1'b1};
|
state_2<=15'b000000000010000;
|
state_1<=12'b000000010000;
|
end
|
end
|
15'b000000000010000: begin
|
12'b000000010000: begin
|
state_2<=15'b000000000100000;
|
err[1]<=sum[15:0];
|
|
of[2]<=of[1];
|
p<={{16{sp[15]}},sp};
|
p<={{16{sp[15]}},sp};
|
a<={{16{~pv[15]}},~pv};
|
a<={{16{~pv[15]}},~pv};
|
cout<=1;
|
cout<=1;
|
|
state_1<=12'b000000100000;
|
end
|
end
|
15'b000000000100000: begin
|
12'b000000100000: begin
|
err[1]<=reg_sum[15:0];
|
err[0]<=sum[15:0];
|
OF[2]<=OF[1];
|
of[1]<=of_addition[0];
|
|
|
state_2<=15'b000000001000000;
|
|
end
|
|
15'b000000001000000: begin
|
|
err[0]<=reg_sum[15:0];
|
|
OF[1]<=reg_OF_addition[0];
|
|
cout<=0;
|
cout<=0;
|
start<=1;
|
start<=1;
|
state_2<=15'b000000010000000;
|
state_1<=12'b000001000000;
|
end
|
end
|
15'b000000010000000: begin
|
12'b000001000000: begin
|
mr_index<=1;
|
mr_index<=1;
|
state_2<=15'b000000100000000;
|
state_1<=12'b000010000000;
|
end
|
end
|
15'b000000100000000: begin
|
12'b000010000000: begin
|
mr_index<=2;
|
mr_index<=2;
|
md_index<=1;
|
md_index<=1;
|
state_2<=15'b000001000000000;
|
state_1<=12'b000100000000;
|
end
|
end
|
15'b000001000000000: begin
|
12'b000100000000: begin
|
mr_index<=0;
|
mr_index<=0;
|
md_index<=0;
|
md_index<=0;
|
start<=0;
|
start<=0;
|
state_2<=15'b000010000000000;
|
p<=product;
|
end
|
|
15'b000010000000000: begin
|
|
p<=reg_product;
|
|
a<=sigma;
|
a<=sigma;
|
state_2<=15'b000100000000000;
|
state_1<=12'b001000000000;
|
|
|
end
|
end
|
15'b000100000000000: begin
|
12'b001000000000: begin
|
//need modi
|
a<=sum;
|
|
sigma<=sum;
|
p<=reg_product;
|
of[3]<=of[4]|of_addition[1];
|
|
of[4]<=of[4]|of_addition[1];
|
state_2<=15'b001000000000000;
|
p<=product;
|
end
|
state_1<=12'b010000000000;
|
15'b001000000000000: begin
|
|
|
end
|
a<=reg_product;
|
12'b010000000000: begin
|
sigma<=reg_sum;
|
a<=sum;
|
OF[3]<=OF[4]|reg_OF_addition[1];
|
of[3]<=of[3]|of_addition[1];
|
OF[4]<=OF[4]|reg_OF_addition[1];
|
p<=product;
|
state_1<=1;
|
state_1<=12'b100000000000;
|
|
end
|
state_2<=15'b010000000000000;
|
12'b100000000000: begin
|
|
un<=sum;
|
end
|
of[3]<=of[3]|of_addition[1];
|
15'b010000000000000: begin
|
state_1<=12'b000000000001;
|
a<=reg_sum; //Kpd*err0-Kd*err1
|
|
p<=sigma;
|
|
OF[3]<=OF[3]|reg_OF_addition[1];
|
|
state_1<=1;
|
|
state_2<=15'b100000000000000;
|
|
end
|
|
15'b100000000000000: begin
|
|
un<=reg_sum;
|
|
OF[3]<=OF[3]|reg_OF_addition[1];
|
|
state_2<=15'b000000000000001;
|
|
wlkp<=0;
|
wlkp<=0;
|
wlki<=0;
|
wlki<=0;
|
wlkd<=0;
|
wlkd<=0;
|
wlsp<=0;
|
wlsp<=0;
|
wlpv<=0;
|
wlpv<=0;
|
rlkpd<=0;
|
rlkpd<=0;
|
rlerr_0<=0;
|
rlerr_0<=0;
|
rlerr_1<=0;
|
rlerr_1<=0;
|
rla<=0;
|
rla<=0;
|
rlsigma<=0;
|
rlsigma<=0;
|
rlOF<=0;
|
rlof<=0;
|
end
|
|
endcase
|
|
end
|
end
|
endcase
|
endcase
|
end
|
end
|
|
|
|
|
wire ready;
|
wire ready;
|
multiplier_16x16bit_pipelined multiplier_16x16bit_pipelined(
|
multiplier_16x16bit_pipelined multiplier_16x16bit_pipelined(
|
i_clk,
|
i_clk,
|
rst,
|
~i_rst,
|
start,
|
start,
|
md,
|
md,
|
mr,
|
mr,
|
product,
|
product,
|
ready
|
ready
|
| Line 440... |
Line 396... |
assign rdata[5]=kpd;
|
assign rdata[5]=kpd;
|
assign rdata[6]=err[0];
|
assign rdata[6]=err[0];
|
assign rdata[7]=err[1];
|
assign rdata[7]=err[1];
|
assign rdata[8]=un[15:0];
|
assign rdata[8]=un[15:0];
|
assign rdata[9]=sigma[15:0];
|
assign rdata[9]=sigma[15:0];
|
assign rdata[10]={11'b0,OF};
|
assign rdata[10]={11'b0,of};
|
`endif
|
`endif
|
|
|
`ifdef wb_32bit
|
`ifdef wb_32bit
|
assign rdata[0]={{16{kp[15]}},kp};
|
assign rdata[0]={{16{kp[15]}},kp};
|
assign rdata[1]={{16{ki[15]}},ki};
|
assign rdata[1]={{16{ki[15]}},ki};
|
| Line 454... |
Line 410... |
assign rdata[5]={{16{kpd[15]}},kpd};
|
assign rdata[5]={{16{kpd[15]}},kpd};
|
assign rdata[6]={{16{err[0][15]}},err[0]};
|
assign rdata[6]={{16{err[0][15]}},err[0]};
|
assign rdata[7]={{16{err[1][15]}},err[1]};
|
assign rdata[7]={{16{err[1][15]}},err[1]};
|
assign rdata[8]=un;
|
assign rdata[8]=un;
|
assign rdata[9]=sigma;
|
assign rdata[9]=sigma;
|
assign rdata[10]={27'b0,OF};
|
assign rdata[10]={27'b0,of};
|
`endif
|
`endif
|
|
|
`ifdef wb_64bit
|
`ifdef wb_64bit
|
assign rdata[0]={{48{kp[15]}},kp};
|
assign rdata[0]={{48{kp[15]}},kp};
|
assign rdata[1]={{48{ki[15]}},ki};
|
assign rdata[1]={{48{ki[15]}},ki};
|
| Line 468... |
Line 424... |
assign rdata[5]={{48{kpd[15]}},kpd};
|
assign rdata[5]={{48{kpd[15]}},kpd};
|
assign rdata[6]={{48{err[0][15]}},err[0]};
|
assign rdata[6]={{48{err[0][15]}},err[0]};
|
assign rdata[7]={{48{err[1][15]}},err[1]};
|
assign rdata[7]={{48{err[1][15]}},err[1]};
|
assign rdata[8]={{32{un[31]}},un};
|
assign rdata[8]={{32{un[31]}},un};
|
assign rdata[9]={{32{sigma[31]}},sigma};
|
assign rdata[9]={{32{sigma[31]}},sigma};
|
assign rdata[10]={59'b0,OF};
|
assign rdata[10]={59'b0,of};
|
`endif
|
`endif
|
|
|
assign rdata[11]=0;
|
assign rdata[11]=0;
|
assign rdata[12]=0;
|
assign rdata[12]=0;
|
assign rdata[13]=0;
|
assign rdata[13]=0;
|
assign rdata[14]=0;
|
assign rdata[14]=0;
|
assign rdata[15]=0;
|
assign rdata[15]=0;
|
|
|
|
|
wire [0:15]rl;
|
wire [0:15]rl;
|
assign rl={5'b0,rlkpd,rlerr_0,rlerr_1,rla,rlsigma,rlOF,5'b0};
|
assign rl={5'b0,rlkpd,rlerr_0,rlerr_1,rla,rlsigma,rlof,5'b0};
|
|
|
wire rack; // wishbone read acknowledged
|
wire rack; // wishbone read acknowledged
|
assign rack=(re&adr_check_1&(~rl[adr_1]))|(re&(~adr_check_1));
|
assign rack=(re&adr_check_1&(~rl[adr_1]))|(re&(~adr_check_1));
|
|
|
assign o_wb_ack=(wack|rack)&i_wb_stb;
|
assign o_wb_ack=(wack|rack)&i_wb_stb;
|
