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

Subversion Repositories pid_controller

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

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


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

Browse

Tools

Subversion Repositories pid_controller

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