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

Subversion Repositories pid_controller

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

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
[15:0]kpd;              read only
[15:0]err[0:1];         read only
[15:0]mr,md;            not accessable
[31:0]p,b;                      not accessable
[31:0]un,sigma;         read only
RS                      write 0 to RS will reset err[0], OF, un and sigma
 
 
 
[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,
                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,
                RS_adr          =       11
)(
input   i_clk,
input   i_rst,  //reset when high
//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
);
 
 
 
reg     [15:0]kp,ki,kd,sp,pv;
reg     wla,wlb;        // write locks
wire    wlRS;
assign  wlRS=wla|wlb;
wire    [0:7]wl={{3{wla}},{2{wlb}},3'h0};
 
reg     wack;   //write acknowledged
 
wire    [2:0]adr; // address for write
`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; // address for read
`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;    // A '1' means address is within the range of adr_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;      // A '1' means address is within the range of adr
`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
reg     RS;
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;
                RS<=0;
        end
        else    begin
                if(wack&&(!i_wb_stb)) wack<=0;
                if(RS)RS<=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 if((adr_1==RS_adr)&&(!wlRS)&&(i_wb_data==0))begin
                                wack<=1;
                                state_0<=0;
                                RS<=1;
                        end
                        else begin
                                wack<=1;
                                state_0<=0;
                        end
                end
                endcase
        end
 
 
 //state machine No.2
reg     [9: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     rla;    // read locks
reg     rlb;
 
 
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     cout;
wire    cin;
wire    [31:0]sum;
wire    [31:0]product;
 
reg     start;  //start signal for multiplier
 
reg     [1:0]mr_index;
reg     [1:0]md_index;
assign  mr=     mr_index==1?kpd:
                mr_index==2?kd:ki;
assign  md=     md_index==2?err[1]:
                md_index==1?err[0]:sum[15:0];
 
 
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;
                wla<=0;
                wlb<=0;
                rla<=0;
                rlb<=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)
                10'b0000000001:  begin
                        if(update_kpd)begin
                                state_1<=10'b0000000010;
                                wla<=1;
                                rla<=1;
                        end
                        else if(update_esu)begin
                                state_1<=10'b0000001000;
                                wla<=1;
                                wlb<=1;
                                rlb<=1;
                        end
                        else if(RS)begin        //start a new sequance of U(n)
                                un<=0;
                                sigma<=0;
                                of<=0;
                                err[0]<=0;
                        end
                end
                10'b0000000010:  begin
                        p<={{16{kp[15]}},kp};
                        a<={{16{kd[15]}},kd};
                        state_1<=10'b0000000100;
                end
                10'b0000000100:  begin
                        kpd<=sum[15:0];
                        wla<=0;
                        rla<=0;
                        of[0]<=of_addition[0];
                        state_1<=10'b0000000001;
                end
                10'b0000001000:  begin
                        p<={{16{sp[15]}},sp};
                        a<={{16{~pv[15]}},~pv};
                        cout<=1;
                        start<=1;                       // start calculate err0 * ki
                        state_1<=10'b0000010000;
                end
                10'b0000010000:  begin
                        err[0]<=sum[15:0];
                        of[1]<=of_addition[0];
                        of[2]<=of[1];
                        p<={{16{~err[0][15]}},~err[0]};
                        a<={31'b0,1'b1};
                        cout<=0;
                        mr_index<=1;            // start calculate err0 * kpd   
                        md_index<=1;
                        state_1<=10'b0000100000;
                end
                10'b0000100000:  begin
                        err[1]<=sum[15:0];
                        mr_index<=2;    // start calculate err1 * kd
                        md_index<=2;
                        state_1<=10'b0001000000;
                end
                10'b0001000000:  begin
                        mr_index<=0;
                        md_index<=0;
                        start<=0;
                        p<=product;     // start calculate err0*ki + sigma_last
                        a<=sigma;
                        state_1<=10'b0010000000;
                end
                10'b0010000000:  begin
                        a<=sum;         // start calculate err0*kpd + sigma_recent
                        sigma<=sum;
                        of[3]<=of[4]|of_addition[1];
                        of[4]<=of[4]|of_addition[1];
                        p<=product;
                        state_1<=10'b0100000000;
                end
                10'b0100000000:  begin
                        a<=sum;         // start calculate err0*kpd + sigma_recent+err1*kd
                        of[3]<=of[3]|of_addition[1];
                        p<=product;
                        state_1<=10'b1000000000;
                end
                10'b1000000000:  begin
                        un<=sum;
                        of[3]<=of[3]|of_addition[1];
                        state_1<=10'b0000000001;
                        wla<=0;
                        wlb<=0;
                        rlb<=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,rla,{4{rlb}},rla|rlb,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=~rlb;
 
 
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	11	m99	`[15:0]kp,ki,kd,sp,pv; can be both read and written`
14			`[15:0]kpd; read only`
15			`[15:0]err[0:1]; read only`
16			`[15:0]mr,md; not accessable`
17			`[31:0]p,b; not accessable`
18			`[31:0]un,sigma; read only`
19			`RS write 0 to RS will reset err[0], OF, un and sigma`
20	2	m99
21
22	11	m99
23	10	m99	`[4:0]of; overflow register, read only through Wishbone interface, address: 0x40`
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	10	m99	`include "PID_defines.v"
37	2	m99
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	11	m99	`adr_wb_nb=16,`
49			`kp_adr = 0,`
50			`ki_adr = 1,`
51			`kd_adr = 2,`
52			`sp_adr = 3,`
53			`pv_adr = 4,`
54			`kpd_adr = 5,`
55			`err_0_adr = 6,`
56			`err_1_adr = 7,`
57			`un_adr = 8,`
58			`sigma_adr = 9,`
59			`of_adr = 10,`
60			`RS_adr = 11`
61	2	m99	`)(`
62			`input i_clk,`
63	12	m99	`input i_rst, //reset when high`
64	10	m99	`//Wishbone Slave port`
65	2	m99	`input i_wb_cyc,`
66			`input i_wb_stb,`
67			`input i_wb_we,`
68			`input [adr_wb_nb-1:0]i_wb_adr,`
69			`input [wb_nb-1:0]i_wb_data,`
70			`output o_wb_ack,`
71			`output [wb_nb-1:0]o_wb_data,`
72
73			`//u(n) output`
74			`output [31:0]o_un,`
75			`output o_valid`
76			`);`
77
78
79	11	m99
80	2	m99	`reg [15:0]kp,ki,kd,sp,pv;`
81	11	m99	`reg wla,wlb; // write locks`
82			`wire wlRS;`
83			`assign wlRS=wla\|wlb;`
84			`wire [0:7]wl={{3{wla}},{2{wlb}},3'h0};`
85	2	m99
86			`reg wack; //write acknowledged`
87
88	11	m99	`wire [2:0]adr; // address for write`
89	2	m99	`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	11	m99	`wire [3:0]adr_1; // address for read`
100	2	m99	`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	11	m99	`wire adr_check_1; // A '1' means address is within the range of adr_1`
119	2	m99	`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	11	m99	`wire adr_check; // A '1' means address is within the range of adr`
130	2	m99	`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	11	m99	`reg RS;`
142	10	m99	`always@(posedge i_clk or posedge i_rst)`
143			`if(i_rst)begin`
144	2	m99	`state_0<=0;`
145			`wack<=0;`
146			`kp<=0;`
147			`ki<=0;`
148			`kd<=0;`
149			`sp<=0;`
150			`pv<=0;`
151	11	m99	`RS<=0;`
152	2	m99	`end`
153			`else begin`
154			`if(wack&&(!i_wb_stb)) wack<=0;`
155	11	m99	`if(RS)RS<=0;`
156	2	m99	`case(state_0)`
157			`0: begin`
158			`if(we&&(!wack)) state_0<=1;`
159			`end`
160			`1: begin`
161			`if(adr_check)begin`
162			`if(!wl[adr])begin`
163			`wack<=1;`
164			`state_0<=0;`
165			`case(adr)`
166			`0: begin`
167			`kp<=i_wb_data[15:0];`
168			`end`
169			`1: begin`
170			`ki<=i_wb_data[15:0];`
171			`end`
172			`2: begin`
173			`kd<=i_wb_data[15:0];`
174			`end`
175			`3: begin`
176			`sp<=i_wb_data[15:0];`
177			`end`
178			`4: begin`
179			`pv<=i_wb_data[15:0];`
180			`end`
181			`endcase`
182
183			`end`
184			`end`
185	11	m99	`else if((adr_1==RS_adr)&&(!wlRS)&&(i_wb_data==0))begin`
186			`wack<=1;`
187			`state_0<=0;`
188			`RS<=1;`
189			`end`
190	2	m99	`else begin`
191	11	m99	`wack<=1;`
192	2	m99	`state_0<=0;`
193			`end`
194			`end`
195			`endcase`
196			`end`
197
198
199			`//state machine No.2`
200	11	m99	`reg [9:0]state_1;`
201	2	m99
202			`wire update_kpd;`
203	8	m99	`assign update_kpd=wack&&(~adr[2])&&(~adr[0])&&adr_check; //adr==0\|\|adr==2`
204	2	m99
205			`wire update_esu; //update e(n), sigma and u(n)`
206			`assign update_esu=wack&&(adr==4)&&adr_check;`
207
208	11	m99	`reg rla; // read locks`
209			`reg rlb;`
210	2	m99
211	11	m99
212	10	m99	`reg [4:0]of;`
213	2	m99	`reg [15:0]kpd;`
214			`reg [15:0]err[0:1];`
215
216			`wire [15:0]mr,md;`
217
218			`reg [31:0]p;`
219			`reg [31:0]a,sigma,un;`
220
221	11	m99	`reg cout;`
222			`wire cin;`
223			`wire [31:0]sum;`
224			`wire [31:0]product;`
225
226	2	m99	`reg start; //start signal for multiplier`
227
228			`reg [1:0]mr_index;`
229	11	m99	`reg [1:0]md_index;`
230	2	m99	`assign mr= mr_index==1?kpd:`
231			`mr_index==2?kd:ki;`
232	11	m99	`assign md= md_index==2?err[1]:`
233			`md_index==1?err[0]:sum[15:0];`
234	2	m99
235
236	10	m99	`wire of_addition[0:1];`
237			`assign of_addition[0]=(p[15]&&a[15]&&(!sum[15]))\|\|((!p[15])&&(!a[15])&&sum[15]);`
238			`assign of_addition[1]=(p[31]&&a[31]&&(!sum[31]))\|\|((!p[31])&&(!a[31])&&sum[31]);`
239	2	m99
240	10	m99	`always@(posedge i_clk or posedge i_rst)`
241			`if(i_rst)begin`
242			`state_1<=12'b000000000001;`
243	11	m99	`wla<=0;`
244			`wlb<=0;`
245	2	m99	`rla<=0;`
246	11	m99	`rlb<=0;`
247	10	m99	`of<=0;`
248	2	m99	`kpd<=0;`
249			`err[0]<=0;`
250			`err[1]<=0;`
251			`p<=0;`
252			`a<=0;`
253			`sigma<=0;`
254			`un<=0;`
255			`start<=0;`
256			`mr_index<=0;`
257			`md_index<=0;`
258			`cout<=0;`
259			`end`
260			`else begin`
261			`case(state_1)`
262	11	m99	`10'b0000000001: begin`
263	10	m99	`if(update_kpd)begin`
264	11	m99	`state_1<=10'b0000000010;`
265			`wla<=1;`
266			`rla<=1;`
267	10	m99	`end`
268			`else if(update_esu)begin`
269	11	m99	`state_1<=10'b0000001000;`
270			`wla<=1;`
271			`wlb<=1;`
272			`rlb<=1;`
273	10	m99	`end`
274	11	m99	`else if(RS)begin //start a new sequance of U(n)`
275			`un<=0;`
276			`sigma<=0;`
277			`of<=0;`
278			`err[0]<=0;`
279			`end`
280	10	m99	`end`
281	11	m99	`10'b0000000010: begin`
282	10	m99	`p<={{16{kp[15]}},kp};`
283			`a<={{16{kd[15]}},kd};`
284	11	m99	`state_1<=10'b0000000100;`
285	10	m99	`end`
286	11	m99	`10'b0000000100: begin`
287	10	m99	`kpd<=sum[15:0];`
288	11	m99	`wla<=0;`
289			`rla<=0;`
290	10	m99	`of[0]<=of_addition[0];`
291	11	m99	`state_1<=10'b0000000001;`
292	10	m99	`end`
293	11	m99	`10'b0000001000: begin`
294	10	m99	`p<={{16{sp[15]}},sp};`
295			`a<={{16{~pv[15]}},~pv};`
296			`cout<=1;`
297	11	m99	`start<=1; // start calculate err0 * ki`
298			`state_1<=10'b0000010000;`
299			`end`
300			`10'b0000010000: begin`
301	10	m99	`err[0]<=sum[15:0];`
302			`of[1]<=of_addition[0];`
303	11	m99	`of[2]<=of[1];`
304			`p<={{16{~err[0][15]}},~err[0]};`
305			`a<={31'b0,1'b1};`
306	10	m99	`cout<=0;`
307	11	m99	`mr_index<=1; // start calculate err0 * kpd`
308			`md_index<=1;`
309			`state_1<=10'b0000100000;`
310			`end`
311			`10'b0000100000: begin`
312			`err[1]<=sum[15:0];`
313			`mr_index<=2; // start calculate err1 * kd`
314			`md_index<=2;`
315			`state_1<=10'b0001000000;`
316	10	m99	`end`
317	11	m99	`10'b0001000000: begin`
318	10	m99	`mr_index<=0;`
319			`md_index<=0;`
320			`start<=0;`
321	11	m99	`p<=product; // start calculate err0*ki + sigma_last`
322	10	m99	`a<=sigma;`
323	11	m99	`state_1<=10'b0010000000;`
324	10	m99	`end`
325	11	m99	`10'b0010000000: begin`
326			`a<=sum; // start calculate err0*kpd + sigma_recent`
327	10	m99	`sigma<=sum;`
328			`of[3]<=of[4]\|of_addition[1];`
329			`of[4]<=of[4]\|of_addition[1];`
330			`p<=product;`
331	11	m99	`state_1<=10'b0100000000;`
332	10	m99	`end`
333	11	m99	`10'b0100000000: begin`
334			`a<=sum; // start calculate err0kpd + sigma_recent+err1kd`
335	10	m99	`of[3]<=of[3]\|of_addition[1];`
336	11	m99	`p<=product;`
337			`state_1<=10'b1000000000;`
338	10	m99	`end`
339	11	m99	`10'b1000000000: begin`
340	10	m99	`un<=sum;`
341			`of[3]<=of[3]\|of_addition[1];`
342	11	m99	`state_1<=10'b0000000001;`
343			`wla<=0;`
344			`wlb<=0;`
345			`rlb<=0;`
346	10	m99	`end`
347	2	m99	`endcase`
348			`end`
349
350
351			`wire ready;`
352			`multiplier_16x16bit_pipelined multiplier_16x16bit_pipelined(`
353			`i_clk,`
354	10	m99	`~i_rst,`
355	2	m99	`start,`
356			`md,`
357			`mr,`
358			`product,`
359			`ready`
360			`);`
361
362			`adder_32bit adder_32bit_0(`
363			`a,`
364			`p,`
365			`cout,`
366			`sum,`
367			`cin`
368			`);`
369
370
371			`wire [wb_nb-1:0]rdata[0:15]; //wishbone read data array`
372			`ifdef wb_16bit
373			`assign rdata[0]=kp;`
374			`assign rdata[1]=ki;`
375			`assign rdata[2]=kd;`
376			`assign rdata[3]=sp;`
377			`assign rdata[4]=pv;`
378			`assign rdata[5]=kpd;`
379			`assign rdata[6]=err[0];`
380			`assign rdata[7]=err[1];`
381			`assign rdata[8]=un[15:0];`
382			`assign rdata[9]=sigma[15:0];`
383	10	m99	`assign rdata[10]={11'b0,of};`
384	2	m99	`endif
385
386			`ifdef wb_32bit
387			`assign rdata[0]={{16{kp[15]}},kp};`
388			`assign rdata[1]={{16{ki[15]}},ki};`
389			`assign rdata[2]={{16{kd[15]}},kd};`
390			`assign rdata[3]={{16{sp[15]}},sp};`
391			`assign rdata[4]={{16{pv[15]}},pv};`
392			`assign rdata[5]={{16{kpd[15]}},kpd};`
393			`assign rdata[6]={{16{err[0][15]}},err[0]};`
394			`assign rdata[7]={{16{err[1][15]}},err[1]};`
395			`assign rdata[8]=un;`
396			`assign rdata[9]=sigma;`
397	10	m99	`assign rdata[10]={27'b0,of};`
398	2	m99	`endif
399
400			`ifdef wb_64bit
401			`assign rdata[0]={{48{kp[15]}},kp};`
402			`assign rdata[1]={{48{ki[15]}},ki};`
403			`assign rdata[2]={{48{kd[15]}},kd};`
404			`assign rdata[3]={{48{sp[15]}},sp};`
405			`assign rdata[4]={{48{pv[15]}},pv};`
406			`assign rdata[5]={{48{kpd[15]}},kpd};`
407			`assign rdata[6]={{48{err[0][15]}},err[0]};`
408			`assign rdata[7]={{48{err[1][15]}},err[1]};`
409			`assign rdata[8]={{32{un[31]}},un};`
410			`assign rdata[9]={{32{sigma[31]}},sigma};`
411	10	m99	`assign rdata[10]={59'b0,of};`
412	2	m99	`endif
413
414			`assign rdata[11]=0;`
415			`assign rdata[12]=0;`
416			`assign rdata[13]=0;`
417			`assign rdata[14]=0;`
418			`assign rdata[15]=0;`
419
420
421			`wire [0:15]rl;`
422	11	m99	`assign rl={5'b0,rla,{4{rlb}},rla\|rlb,5'b0};`
423	2	m99
424			`wire rack; // wishbone read acknowledged`
425			`assign rack=(re&adr_check_1&(~rl[adr_1]))\|(re&(~adr_check_1));`
426
427			`assign o_wb_ack=(wack\|rack)&i_wb_stb;`
428
429			`assign o_wb_data=adr_check_1?rdata[adr_1]:0;`
430			`assign o_un=un;`
431	11	m99	`assign o_valid=~rlb;`
432	2	m99
433
434			`endmodule`

Browse

Tools

Subversion Repositories pid_controller

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