URL https://opencores.org/ocsvn/pid_controller/pid_controller/trunk

Subversion Repositories pid_controller

[/] [pid_controller/] [trunk/] [RTL/] [PID.v] - Blame information for rev 6

Go to most recent revision | Details | Compare with Previous | View Log


/* PID controller
Author: Zhu Xu
Email: m99a1@yahoo.cn
 
sigma=Ki*e(n)+sigma
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.
Address width of Wishbone slave port can be can be modified by changing parameter adr_wb_nb.
 
Wishbone compliant
Work as Wishbone slave, support Classic standard SINGLE/BLOCK READ/WRITE Cycle
 
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]kpd;              read only through Wishbone interface, address: 0x14
[15:0]err[0:1];         read only through Wishbone interface, address: 0x18, 0x1c
[15:0]mr,md;            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
 
 
[4:0]OF;                        overflow register, read only through Wishbone interface, address: 0x28
OF[0]==1        :       kpd overflow
OF[1]==1        :       err[0] overflow
OF[2]==1        :       err[1] overflow
OF[3]==1        :       un overflow
OF[4]==1        :       sigma overflow
[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
 
 
 
*/
 
`include "PID_defines.v"
 
module  PID #(
`ifdef wb_16bit
parameter       wb_nb=16,
`endif
`ifdef wb_32bit
parameter       wb_nb=32,
`endif
`ifdef wb_64bit
parameter       wb_nb=64,
`endif
                adr_wb_nb=16
)(
//Wishbone Slave Interface
input   i_clk,
input   i_rst,
input   i_wb_cyc,
input   i_wb_stb,
input   i_wb_we,
input   [adr_wb_nb-1:0]i_wb_adr,
input   [wb_nb-1:0]i_wb_data,
output  o_wb_ack,
output  [wb_nb-1:0]o_wb_data,
 
//u(n) output
output  [31:0]o_un,
output  o_valid
);
 
parameter       kp_adr          =       0,
                ki_adr          =       1,
                kd_adr          =       2,
                sp_adr          =       3,
                pv_adr          =       4,
                kpd_adr         =       5,
                err_0_adr               =       6,
                err_1_adr               =       7,
                un_adr          =       8,
                sigma_adr       =       9,
                OF_adr          =       10;
 
wire rst;
assign  rst=~i_rst;
 
reg     [15:0]kp,ki,kd,sp,pv;
reg     wlkp,wlki,wlkd,wlsp,wlpv;
 
wire    [0:7]wl={wlkp,wlki,wlkd,wlsp,wlpv,3'h0};
 
reg     wack;   //write acknowledged
 
wire    [2:0]adr;
`ifdef wb_16bit
assign  adr=i_wb_adr[3:1];
`endif
`ifdef wb_32bit
assign  adr=i_wb_adr[4:2];
`endif
`ifdef wb_64bit
assign  adr=i_wb_adr[5:3];
`endif
 
wire    [3:0]adr_1;
`ifdef  wb_32bit
assign  adr_1=i_wb_adr[5:2];
`endif
`ifdef  wb_16bit
assign  adr_1=i_wb_adr[4:1];
`endif
`ifdef  wb_64bit
assign  adr_1=i_wb_adr[6:3];
`endif
 
 
wire    we;     // write enable
assign  we=i_wb_cyc&i_wb_we&i_wb_stb;
wire    re;     //read enable
assign  re=i_wb_cyc&(~i_wb_we)&i_wb_stb;
 
reg     state_0;  //state machine No.1's state register
 
wire    adr_check_1;
`ifdef  wb_32bit
assign  adr_check_1=i_wb_adr[adr_wb_nb-1:6]==0;
`endif
`ifdef  wb_16bit
assign  adr_check_1=i_wb_adr[adr_wb_nb-1:5]==0;
`endif
`ifdef  wb_64bit
assign  adr_check_1=i_wb_adr[adr_wb_nb-1:7]==0;
`endif
 
wire    adr_check;      //check address's correctness
`ifdef wb_16bit
assign  adr_check=i_wb_adr[4]==0&&adr_check_1;
`endif
`ifdef wb_32bit
assign  adr_check=i_wb_adr[5]==0&&adr_check_1;
`endif
`ifdef wb_64bit
assign  adr_check=i_wb_adr[6]==0&&adr_check_1;
`endif
 
 //state machine No.1
always@(posedge i_clk or negedge rst)
        if(!rst)begin
                state_0<=0;
                wack<=0;
                kp<=0;
                ki<=0;
                kd<=0;
                sp<=0;
                pv<=0;
 
        end
        else    begin
                if(wack&&(!i_wb_stb)) wack<=0;
                case(state_0)
                0:       begin
                        if(we&&(!wack)) state_0<=1;
                end
                1:      begin
                        if(adr_check)begin
                                if(!wl[adr])begin
                                        wack<=1;
                                        state_0<=0;
                                        case(adr)
                                        0:       begin
                                                kp<=i_wb_data[15:0];
                                        end
                                        1:      begin
                                                ki<=i_wb_data[15:0];
                                        end
                                        2:      begin
                                                kd<=i_wb_data[15:0];
                                        end
                                        3:      begin
                                                sp<=i_wb_data[15:0];
                                        end
                                        4:      begin
                                                pv<=i_wb_data[15:0];
                                        end
                                        endcase
 
                                end
                        end
                        else begin
                                 wack<=1;
                                state_0<=0;
                        end
                end
                endcase
        end
 
 
 //state machine No.2
reg     [14:0]state_2;
reg     state_1;
 
wire    update_kpd;
assign  update_kpd=wack&&(~adr[2])&(~adr[0])&&adr_check; //adr==0||adr==2
 
wire    update_esu;     //update e(n), sigma and u(n)
assign  update_esu=wack&&(adr==4)&&adr_check;
 
reg     rlkpd;
reg     rlerr_0;
reg     rlerr_1;
reg     rla;
reg     rlsigma;
reg     rlOF;
 
reg     [4:0]OF;
reg     [15:0]kpd;
reg     [15:0]err[0:1];
 
wire    [15:0]mr,md;
 
reg     [31:0]p;
reg     [31:0]a,sigma,un;
 
reg     start;  //start signal for multiplier
 
reg     [1:0]mr_index;
reg     md_index;
assign  mr=     mr_index==1?kpd:
                mr_index==2?kd:ki;
assign  md=     md_index?err[1]:err[0];
 
reg     cout;
wire    cin;
wire    [31:0]sum;
wire    [31:0]product;
 
wire    OF_addition[0:1];
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]);
 
 
 
reg     [31:0]reg_sum;
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;
                wlki<=0;
                wlkd<=0;
                wlsp<=0;
                wlpv<=0;
                rlkpd<=0;
                rlerr_0<=0;
                rlerr_1<=0;
                rla<=0;
                rlsigma<=0;
                rlOF<=0;
                OF<=0;
                kpd<=0;
                err[0]<=0;
                err[1]<=0;
                p<=0;
                a<=0;
                sigma<=0;
                un<=0;
                start<=0;
                mr_index<=0;
                md_index<=0;
                cout<=0;
        end
        else begin
                case(state_1)
                        1:      state_1<=0;
                        0:begin
                                case(state_2)
                                        15'b0000000000001:      begin
                                                if(update_kpd)begin
                                                        state_2<=15'b000000000000010;
                                                        wlkp<=1;
                                                        wlkd<=1;
                                                        wlpv<=1;
                                                        rlkpd<=1;
                                                        rlOF<=1;
                                                end
                                                else if(update_esu)begin
                                                        state_2<=15'b00000000001000;
                                                        wlkp<=1;
                                                        wlki<=1;
                                                        wlkd<=1;
                                                        wlsp<=1;
                                                        wlpv<=1;
                                                        rlkpd<=1;
                                                        rlerr_0<=1;
                                                        rlerr_1<=1;
                                                        rla<=1;
                                                        rlsigma<=1;
                                                        rlOF<=1;
                                                end
                                        end
                                        15'b000000000000010:    begin
                                                p<={{16{kp[15]}},kp};
                                                a<={{16{kd[15]}},kd};
                                                state_2<=15'b000000000000100;
                                                state_1<=1;
                                        end
                                        15'b000000000000100:    begin
                                                kpd<=reg_sum[15:0];
                                                wlkp<=0;
                                                wlkd<=0;
                                                wlpv<=0;
                                                rlkpd<=0;
                                                rlOF<=0;
                                                OF[0]<=reg_OF_addition[0];
                                                state_2<=15'b000000000000001;
                                        end
                                        15'b000000000001000:    begin
                                                p<={{16{~err[0][15]}},~err[0]};
                                                a<={31'b0,1'b1};
                                                state_2<=15'b000000000010000;
                                        end
                                        15'b000000000010000:    begin
                                                state_2<=15'b000000000100000;
                                                p<={{16{sp[15]}},sp};
                                                a<={{16{~pv[15]}},~pv};
                                                cout<=1;
                                        end
                                        15'b000000000100000:    begin
                                                err[1]<=reg_sum[15:0];
                                                OF[2]<=OF[1];
 
                                                state_2<=15'b000000001000000;
                                        end
                                        15'b000000001000000:    begin
                                                err[0]<=reg_sum[15:0];
                                                OF[1]<=reg_OF_addition[0];
                                                cout<=0;
                                                start<=1;
                                                state_2<=15'b000000010000000;
                                        end
                                        15'b000000010000000:    begin
                                                mr_index<=1;
                                                state_2<=15'b000000100000000;
                                        end
                                        15'b000000100000000:    begin
                                                mr_index<=2;
                                                md_index<=1;
                                                state_2<=15'b000001000000000;
                                        end
                                        15'b000001000000000:    begin
                                                mr_index<=0;
                                                md_index<=0;
                                                start<=0;
                                                state_2<=15'b000010000000000;
                                        end
                                        15'b000010000000000:    begin
                                                p<=reg_product;
                                                a<=sigma;
                                                state_2<=15'b000100000000000;
 
                                        end
                                        15'b000100000000000:    begin
                                                //need modi
 
                                                p<=reg_product;
 
                                                state_2<=15'b001000000000000;
                                        end
                                        15'b001000000000000:    begin
 
                                                a<=reg_product;
                                                sigma<=reg_sum;
                                                OF[3]<=OF[4]|reg_OF_addition[1];
                                                OF[4]<=OF[4]|reg_OF_addition[1];
                                                state_1<=1;
 
                                                state_2<=15'b010000000000000;
 
                                        end
                                        15'b010000000000000:    begin
                                                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;
                                                wlki<=0;
                                                wlkd<=0;
                                                wlsp<=0;
                                                wlpv<=0;
                                                rlkpd<=0;
                                                rlerr_0<=0;
                                                rlerr_1<=0;
                                                rla<=0;
                                                rlsigma<=0;
                                                rlOF<=0;
                                        end
                                endcase
                        end
                endcase
        end
 
 
wire    ready;
multiplier_16x16bit_pipelined   multiplier_16x16bit_pipelined(
i_clk,
rst,
start,
md,
mr,
product,
ready
);
 
adder_32bit     adder_32bit_0(
a,
p,
cout,
sum,
cin
);
 
 
wire    [wb_nb-1:0]rdata[0:15];   //wishbone read data array
`ifdef  wb_16bit
assign  rdata[0]=kp;
assign  rdata[1]=ki;
assign  rdata[2]=kd;
assign  rdata[3]=sp;
assign  rdata[4]=pv;
assign  rdata[5]=kpd;
assign  rdata[6]=err[0];
assign  rdata[7]=err[1];
assign  rdata[8]=un[15:0];
assign  rdata[9]=sigma[15:0];
assign  rdata[10]={11'b0,OF};
`endif
 
`ifdef  wb_32bit
assign  rdata[0]={{16{kp[15]}},kp};
assign  rdata[1]={{16{ki[15]}},ki};
assign  rdata[2]={{16{kd[15]}},kd};
assign  rdata[3]={{16{sp[15]}},sp};
assign  rdata[4]={{16{pv[15]}},pv};
assign  rdata[5]={{16{kpd[15]}},kpd};
assign  rdata[6]={{16{err[0][15]}},err[0]};
assign  rdata[7]={{16{err[1][15]}},err[1]};
assign  rdata[8]=un;
assign  rdata[9]=sigma;
assign  rdata[10]={27'b0,OF};
`endif
 
`ifdef  wb_64bit
assign  rdata[0]={{48{kp[15]}},kp};
assign  rdata[1]={{48{ki[15]}},ki};
assign  rdata[2]={{48{kd[15]}},kd};
assign  rdata[3]={{48{sp[15]}},sp};
assign  rdata[4]={{48{pv[15]}},pv};
assign  rdata[5]={{48{kpd[15]}},kpd};
assign  rdata[6]={{48{err[0][15]}},err[0]};
assign  rdata[7]={{48{err[1][15]}},err[1]};
assign  rdata[8]={{32{un[31]}},un};
assign  rdata[9]={{32{sigma[31]}},sigma};
assign  rdata[10]={59'b0,OF};
`endif
 
assign  rdata[11]=0;
assign  rdata[12]=0;
assign  rdata[13]=0;
assign  rdata[14]=0;
assign  rdata[15]=0;
 
 
wire    [0:15]rl;
assign  rl={5'b0,rlkpd,rlerr_0,rlerr_1,rla,rlsigma,rlOF,5'b0};
 
wire    rack;   // wishbone read acknowledged
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_data=adr_check_1?rdata[adr_1]:0;
assign  o_un=un;
assign  o_valid=~rla;
 
 
endmodule

Line No.	Rev	Author	Line
1	2	m99	`/* PID controller`
2			`Author: Zhu Xu`
3			`Email: m99a1@yahoo.cn`
4
5			`sigma=Ki*e(n)+sigma`
6			`u(n)=(Kp+Kd)e(n)+sigma+Kd(-e(n-1))`
7
8			`Data width of Wishbone slave port can be can be toggled between 64-bit, 32-bit and 16-bit.`
9			`Address width of Wishbone slave port can be can be modified by changing parameter adr_wb_nb.`
10
11			`Wishbone compliant`
12			`Work as Wishbone slave, support Classic standard SINGLE/BLOCK READ/WRITE Cycle`
13
14			`registers or wires`
15			`[15:0]kp,ki,kd,sp,pv; can be both read and written through Wishbone interface, address: 0x0, 0x4, 0x8, 0xc, 0x10`
16			`[15:0]kpd; read only through Wishbone interface, address: 0x14`
17			`[15:0]err[0:1]; read only through Wishbone interface, address: 0x18, 0x1c`
18			`[15:0]mr,md; not accessable through Wishbone interface`
19			`[31:0]p,b; not accessable through Wishbone interface`
20			`[31:0]un,sigma; read only through Wishbone interface, address: 0x20, 0x24`
21
22
23	4	m99	`[4:0]OF; overflow register, read only through Wishbone interface, address: 0x28`
24			`OF[0]==1 : kpd overflow`
25			`OF[1]==1 : err[0] overflow`
26			`OF[2]==1 : err[1] overflow`
27			`OF[3]==1 : un overflow`
28			`OF[4]==1 : sigma overflow`
29	2	m99	`[0:15]rl; read lock, when asserted corelated reagister can not be read through Wishbone interface`
30			`[0:7]wl; write lock, when asserted corelated reagister can not be written through Wishbone interface`
31
32
33
34			`*/`
35
36			`include "PID_defines.v"
37
38			`module PID #(`
39			`ifdef wb_16bit
40			`parameter wb_nb=16,`
41			`endif
42			`ifdef wb_32bit
43			`parameter wb_nb=32,`
44			`endif
45			`ifdef wb_64bit
46			`parameter wb_nb=64,`
47			`endif
48			`adr_wb_nb=16`
49			`)(`
50			`//Wishbone Slave Interface`
51			`input i_clk,`
52			`input i_rst,`
53			`input i_wb_cyc,`
54			`input i_wb_stb,`
55			`input i_wb_we,`
56			`input [adr_wb_nb-1:0]i_wb_adr,`
57			`input [wb_nb-1:0]i_wb_data,`
58			`output o_wb_ack,`
59			`output [wb_nb-1:0]o_wb_data,`
60
61			`//u(n) output`
62			`output [31:0]o_un,`
63			`output o_valid`
64			`);`
65
66			`parameter kp_adr = 0,`
67			`ki_adr = 1,`
68			`kd_adr = 2,`
69			`sp_adr = 3,`
70			`pv_adr = 4,`
71			`kpd_adr = 5,`
72			`err_0_adr = 6,`
73			`err_1_adr = 7,`
74			`un_adr = 8,`
75			`sigma_adr = 9,`
76	4	m99	`OF_adr = 10;`
77	2	m99
78			`wire rst;`
79			`assign rst=~i_rst;`
80
81			`reg [15:0]kp,ki,kd,sp,pv;`
82			`reg wlkp,wlki,wlkd,wlsp,wlpv;`
83
84			`wire [0:7]wl={wlkp,wlki,wlkd,wlsp,wlpv,3'h0};`
85
86			`reg wack; //write acknowledged`
87
88			`wire [2:0]adr;`
89			`ifdef wb_16bit
90			`assign adr=i_wb_adr[3:1];`
91			`endif
92			`ifdef wb_32bit
93			`assign adr=i_wb_adr[4:2];`
94			`endif
95			`ifdef wb_64bit
96			`assign adr=i_wb_adr[5:3];`
97			`endif
98
99			`wire [3:0]adr_1;`
100			`ifdef wb_32bit
101			`assign adr_1=i_wb_adr[5:2];`
102			`endif
103			`ifdef wb_16bit
104			`assign adr_1=i_wb_adr[4:1];`
105			`endif
106			`ifdef wb_64bit
107			`assign adr_1=i_wb_adr[6:3];`
108			`endif
109
110
111			`wire we; // write enable`
112			`assign we=i_wb_cyc&i_wb_we&i_wb_stb;`
113			`wire re; //read enable`
114			`assign re=i_wb_cyc&(~i_wb_we)&i_wb_stb;`
115
116			`reg state_0; //state machine No.1's state register`
117
118			`wire adr_check_1;`
119			`ifdef wb_32bit
120			`assign adr_check_1=i_wb_adr[adr_wb_nb-1:6]==0;`
121			`endif
122			`ifdef wb_16bit
123			`assign adr_check_1=i_wb_adr[adr_wb_nb-1:5]==0;`
124			`endif
125			`ifdef wb_64bit
126			`assign adr_check_1=i_wb_adr[adr_wb_nb-1:7]==0;`
127			`endif
128
129			`wire adr_check; //check address's correctness`
130			`ifdef wb_16bit
131			`assign adr_check=i_wb_adr[4]==0&&adr_check_1;`
132			`endif
133			`ifdef wb_32bit
134			`assign adr_check=i_wb_adr[5]==0&&adr_check_1;`
135			`endif
136			`ifdef wb_64bit
137			`assign adr_check=i_wb_adr[6]==0&&adr_check_1;`
138			`endif
139
140			`//state machine No.1`
141			`always@(posedge i_clk or negedge rst)`
142			`if(!rst)begin`
143			`state_0<=0;`
144			`wack<=0;`
145			`kp<=0;`
146			`ki<=0;`
147			`kd<=0;`
148			`sp<=0;`
149			`pv<=0;`
150
151			`end`
152			`else begin`
153			`if(wack&&(!i_wb_stb)) wack<=0;`
154			`case(state_0)`
155			`0: begin`
156			`if(we&&(!wack)) state_0<=1;`
157			`end`
158			`1: begin`
159			`if(adr_check)begin`
160			`if(!wl[adr])begin`
161			`wack<=1;`
162			`state_0<=0;`
163			`case(adr)`
164			`0: begin`
165			`kp<=i_wb_data[15:0];`
166			`end`
167			`1: begin`
168			`ki<=i_wb_data[15:0];`
169			`end`
170			`2: begin`
171			`kd<=i_wb_data[15:0];`
172			`end`
173			`3: begin`
174			`sp<=i_wb_data[15:0];`
175			`end`
176			`4: begin`
177			`pv<=i_wb_data[15:0];`
178			`end`
179			`endcase`
180
181			`end`
182			`end`
183			`else begin`
184			`wack<=1;`
185			`state_0<=0;`
186			`end`
187			`end`
188			`endcase`
189			`end`
190
191
192			`//state machine No.2`
193			`reg [14:0]state_2;`
194			`reg state_1;`
195
196			`wire update_kpd;`
197			`assign update_kpd=wack&&(~adr[2])&(~adr[0])&&adr_check; //adr==0\|\|adr==2`
198
199			`wire update_esu; //update e(n), sigma and u(n)`
200			`assign update_esu=wack&&(adr==4)&&adr_check;`
201
202			`reg rlkpd;`
203			`reg rlerr_0;`
204			`reg rlerr_1;`
205			`reg rla;`
206			`reg rlsigma;`
207	4	m99	`reg rlOF;`
208	2	m99
209	4	m99	`reg [4:0]OF;`
210	2	m99	`reg [15:0]kpd;`
211			`reg [15:0]err[0:1];`
212
213			`wire [15:0]mr,md;`
214
215			`reg [31:0]p;`
216			`reg [31:0]a,sigma,un;`
217
218			`reg start; //start signal for multiplier`
219
220			`reg [1:0]mr_index;`
221			`reg md_index;`
222			`assign mr= mr_index==1?kpd:`
223			`mr_index==2?kd:ki;`
224			`assign md= md_index?err[1]:err[0];`
225
226			`reg cout;`
227			`wire cin;`
228			`wire [31:0]sum;`
229			`wire [31:0]product;`
230
231	4	m99	`wire OF_addition[0:1];`
232			`assign OF_addition[0]=(p[15]&&a[15]&&(!sum[15]))\|\|((!p[15])&&(!a[15])&&sum[15]);`
233			`assign OF_addition[1]=(p[31]&&a[31]&&(!sum[31]))\|\|((!p[31])&&(!a[31])&&sum[31]);`
234	2	m99
235
236
237			`reg [31:0]reg_sum;`
238			`reg [31:0]reg_product;`
239	4	m99	`reg reg_OF_addition[0:1];`
240	2	m99
241			`always@(posedge i_clk)begin`
242			`reg_sum<=sum;`
243	4	m99	`reg_OF_addition[0]<=OF_addition[0];`
244			`reg_OF_addition[1]<=OF_addition[1];`
245	2	m99	`reg_product<=product;`
246			`end`
247
248			`always@(posedge i_clk or negedge rst)`
249			`if(!rst)begin`
250			`state_1<=0;`
251			`state_2<=15'b000000000000001;`
252			`wlkp<=0;`
253			`wlki<=0;`
254			`wlkd<=0;`
255			`wlsp<=0;`
256			`wlpv<=0;`
257			`rlkpd<=0;`
258			`rlerr_0<=0;`
259			`rlerr_1<=0;`
260			`rla<=0;`
261			`rlsigma<=0;`
262	4	m99	`rlOF<=0;`
263			`OF<=0;`
264	2	m99	`kpd<=0;`
265			`err[0]<=0;`
266			`err[1]<=0;`
267			`p<=0;`
268			`a<=0;`
269			`sigma<=0;`
270			`un<=0;`
271			`start<=0;`
272			`mr_index<=0;`
273			`md_index<=0;`
274			`cout<=0;`
275			`end`
276			`else begin`
277			`case(state_1)`
278			`1: state_1<=0;`
279			`0:begin`
280			`case(state_2)`
281			`15'b0000000000001: begin`
282			`if(update_kpd)begin`
283			`state_2<=15'b000000000000010;`
284			`wlkp<=1;`
285			`wlkd<=1;`
286			`wlpv<=1;`
287			`rlkpd<=1;`
288	4	m99	`rlOF<=1;`
289	2	m99	`end`
290			`else if(update_esu)begin`
291			`state_2<=15'b00000000001000;`
292			`wlkp<=1;`
293			`wlki<=1;`
294			`wlkd<=1;`
295			`wlsp<=1;`
296			`wlpv<=1;`
297			`rlkpd<=1;`
298			`rlerr_0<=1;`
299			`rlerr_1<=1;`
300			`rla<=1;`
301			`rlsigma<=1;`
302	4	m99	`rlOF<=1;`
303	2	m99	`end`
304			`end`
305			`15'b000000000000010: begin`
306			`p<={{16{kp[15]}},kp};`
307			`a<={{16{kd[15]}},kd};`
308			`state_2<=15'b000000000000100;`
309			`state_1<=1;`
310			`end`
311			`15'b000000000000100: begin`
312			`kpd<=reg_sum[15:0];`
313			`wlkp<=0;`
314			`wlkd<=0;`
315			`wlpv<=0;`
316			`rlkpd<=0;`
317	4	m99	`rlOF<=0;`
318			`OF[0]<=reg_OF_addition[0];`
319	2	m99	`state_2<=15'b000000000000001;`
320			`end`
321			`15'b000000000001000: begin`
322			`p<={{16{~err[0][15]}},~err[0]};`
323			`a<={31'b0,1'b1};`
324			`state_2<=15'b000000000010000;`
325			`end`
326			`15'b000000000010000: begin`
327			`state_2<=15'b000000000100000;`
328			`p<={{16{sp[15]}},sp};`
329			`a<={{16{~pv[15]}},~pv};`
330			`cout<=1;`
331			`end`
332			`15'b000000000100000: begin`
333			`err[1]<=reg_sum[15:0];`
334	4	m99	`OF[2]<=OF[1];`
335	2	m99
336			`state_2<=15'b000000001000000;`
337			`end`
338			`15'b000000001000000: begin`
339			`err[0]<=reg_sum[15:0];`
340	4	m99	`OF[1]<=reg_OF_addition[0];`
341	2	m99	`cout<=0;`
342			`start<=1;`
343			`state_2<=15'b000000010000000;`
344			`end`
345			`15'b000000010000000: begin`
346			`mr_index<=1;`
347			`state_2<=15'b000000100000000;`
348			`end`
349			`15'b000000100000000: begin`
350			`mr_index<=2;`
351			`md_index<=1;`
352			`state_2<=15'b000001000000000;`
353			`end`
354			`15'b000001000000000: begin`
355			`mr_index<=0;`
356			`md_index<=0;`
357			`start<=0;`
358			`state_2<=15'b000010000000000;`
359			`end`
360			`15'b000010000000000: begin`
361			`p<=reg_product;`
362			`a<=sigma;`
363			`state_2<=15'b000100000000000;`
364
365			`end`
366			`15'b000100000000000: begin`
367			`//need modi`
368
369			`p<=reg_product;`
370
371			`state_2<=15'b001000000000000;`
372			`end`
373			`15'b001000000000000: begin`
374
375			`a<=reg_product;`
376			`sigma<=reg_sum;`
377	4	m99	`OF[3]<=OF[4]\|reg_OF_addition[1];`
378			`OF[4]<=OF[4]\|reg_OF_addition[1];`
379	2	m99	`state_1<=1;`
380
381			`state_2<=15'b010000000000000;`
382
383			`end`
384			`15'b010000000000000: begin`
385			`a<=reg_sum; //Kpderr0-Kderr1`
386			`p<=sigma;`
387	4	m99	`OF[3]<=OF[3]\|reg_OF_addition[1];`
388	2	m99	`state_1<=1;`
389			`state_2<=15'b100000000000000;`
390			`end`
391			`15'b100000000000000: begin`
392			`un<=reg_sum;`
393	4	m99	`OF[3]<=OF[3]\|reg_OF_addition[1];`
394	2	m99	`state_2<=15'b000000000000001;`
395			`wlkp<=0;`
396			`wlki<=0;`
397			`wlkd<=0;`
398			`wlsp<=0;`
399			`wlpv<=0;`
400			`rlkpd<=0;`
401			`rlerr_0<=0;`
402			`rlerr_1<=0;`
403			`rla<=0;`
404			`rlsigma<=0;`
405	4	m99	`rlOF<=0;`
406	2	m99	`end`
407			`endcase`
408			`end`
409			`endcase`
410			`end`
411
412
413			`wire ready;`
414			`multiplier_16x16bit_pipelined multiplier_16x16bit_pipelined(`
415			`i_clk,`
416			`rst,`
417			`start,`
418			`md,`
419			`mr,`
420			`product,`
421			`ready`
422			`);`
423
424			`adder_32bit adder_32bit_0(`
425			`a,`
426			`p,`
427			`cout,`
428			`sum,`
429			`cin`
430			`);`
431
432
433			`wire [wb_nb-1:0]rdata[0:15]; //wishbone read data array`
434			`ifdef wb_16bit
435			`assign rdata[0]=kp;`
436			`assign rdata[1]=ki;`
437			`assign rdata[2]=kd;`
438			`assign rdata[3]=sp;`
439			`assign rdata[4]=pv;`
440			`assign rdata[5]=kpd;`
441			`assign rdata[6]=err[0];`
442			`assign rdata[7]=err[1];`
443			`assign rdata[8]=un[15:0];`
444			`assign rdata[9]=sigma[15:0];`
445	4	m99	`assign rdata[10]={11'b0,OF};`
446	2	m99	`endif
447
448			`ifdef wb_32bit
449			`assign rdata[0]={{16{kp[15]}},kp};`
450			`assign rdata[1]={{16{ki[15]}},ki};`
451			`assign rdata[2]={{16{kd[15]}},kd};`
452			`assign rdata[3]={{16{sp[15]}},sp};`
453			`assign rdata[4]={{16{pv[15]}},pv};`
454			`assign rdata[5]={{16{kpd[15]}},kpd};`
455			`assign rdata[6]={{16{err[0][15]}},err[0]};`
456			`assign rdata[7]={{16{err[1][15]}},err[1]};`
457			`assign rdata[8]=un;`
458			`assign rdata[9]=sigma;`
459	4	m99	`assign rdata[10]={27'b0,OF};`
460	2	m99	`endif
461
462			`ifdef wb_64bit
463			`assign rdata[0]={{48{kp[15]}},kp};`
464			`assign rdata[1]={{48{ki[15]}},ki};`
465			`assign rdata[2]={{48{kd[15]}},kd};`
466			`assign rdata[3]={{48{sp[15]}},sp};`
467			`assign rdata[4]={{48{pv[15]}},pv};`
468			`assign rdata[5]={{48{kpd[15]}},kpd};`
469			`assign rdata[6]={{48{err[0][15]}},err[0]};`
470			`assign rdata[7]={{48{err[1][15]}},err[1]};`
471			`assign rdata[8]={{32{un[31]}},un};`
472			`assign rdata[9]={{32{sigma[31]}},sigma};`
473	4	m99	`assign rdata[10]={59'b0,OF};`
474	2	m99	`endif
475
476			`assign rdata[11]=0;`
477			`assign rdata[12]=0;`
478			`assign rdata[13]=0;`
479			`assign rdata[14]=0;`
480			`assign rdata[15]=0;`
481
482
483			`wire [0:15]rl;`
484	4	m99	`assign rl={5'b0,rlkpd,rlerr_0,rlerr_1,rla,rlsigma,rlOF,5'b0};`
485	2	m99
486			`wire rack; // wishbone read acknowledged`
487			`assign rack=(re&adr_check_1&(~rl[adr_1]))\|(re&(~adr_check_1));`
488
489			`assign o_wb_ack=(wack\|rack)&i_wb_stb;`
490
491			`assign o_wb_data=adr_check_1?rdata[adr_1]:0;`
492			`assign o_un=un;`
493			`assign o_valid=~rla;`
494
495
496			`endmodule`

Browse

Tools

Subversion Repositories pid_controller

[/] [pid_controller/] [trunk/] [RTL/] [PID.v] - Blame information for rev 6