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

Subversion Repositories pid_controller

[/] [pid_controller/] [trunk/] [RTL/] [PID.v] - Diff between revs 10 and 11

Go to most recent revision | Show entire file | Details | Blame | View Log

Rev 10 Rev 11
Line 8... 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, 0x12, 0x16
 
[15:0]kp,ki,kd,sp,pv;   can be both read and written
 
[15:0]kpd;              read only through Wishbone interface, address: 0x20
 
[15:0]kpd;              read only
 
[15:0]err[0:1];         read only through Wishbone interface, address: 0x24, 0x28
 
[15:0]err[0:1];         read only
 
[15:0]mr,md;            not accessable through Wishbone interface
 
[15:0]mr,md;            not accessable
 
[31:0]p,b;                      not accessable through Wishbone interface
 
[31:0]p,b;                      not accessable
 
[31:0]un,sigma;         read only through Wishbone interface, address: 0x32, 0x36
 
[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
 
[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
Line 41... Line 43... 
parameter       wb_nb=32,
 
parameter       wb_nb=32,
 
`endif
 
`endif
 
`ifdef wb_64bit
 
`ifdef wb_64bit
 
parameter       wb_nb=64,
 
parameter       wb_nb=64,
 
`endif
 
`endif
 
                adr_wb_nb=16
 
                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_clk,
 
input   i_rst,  //reset when low
 
input   i_rst,  //reset when low
 
//Wishbone Slave port
 
//Wishbone Slave port
 
input   i_wb_cyc,
 
input   i_wb_cyc,
Line 59... Line 73... 
//u(n) output
 
//u(n) output
 
output  [31:0]o_un,
 
output  [31:0]o_un,
 
output  o_valid
 
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     [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
 
reg     wack;   //write acknowledged
 
 
 
 
 
wire    [2:0]adr;
 
wire    [2:0]adr; // address for write
 
`ifdef wb_16bit
 
`ifdef wb_16bit
 
assign  adr=i_wb_adr[3:1];
 
assign  adr=i_wb_adr[3:1];
 
`endif
 
`endif
 
`ifdef wb_32bit
 
`ifdef wb_32bit
 
assign  adr=i_wb_adr[4:2];
 
assign  adr=i_wb_adr[4:2];
 
`endif
 
`endif
 
`ifdef wb_64bit
 
`ifdef wb_64bit
 
assign  adr=i_wb_adr[5:3];
 
assign  adr=i_wb_adr[5:3];
 
`endif
 
`endif
 
 
 
 
 
wire    [3:0]adr_1;
 
wire    [3:0]adr_1; // address for read
 
`ifdef  wb_32bit
 
`ifdef  wb_32bit
 
assign  adr_1=i_wb_adr[5:2];
 
assign  adr_1=i_wb_adr[5:2];
 
`endif
 
`endif
 
`ifdef  wb_16bit
 
`ifdef  wb_16bit
 
assign  adr_1=i_wb_adr[4:1];
 
assign  adr_1=i_wb_adr[4:1];
Line 108... Line 113... 
wire    re;     //read enable
 
wire    re;     //read enable
 
assign  re=i_wb_cyc&(~i_wb_we)&i_wb_stb;
 
assign  re=i_wb_cyc&(~i_wb_we)&i_wb_stb;
 
 
 
 
 
reg     state_0;  //state machine No.1's state register
 
reg     state_0;  //state machine No.1's state register
 
 
 
 
 
wire    adr_check_1;
 
wire    adr_check_1;    // A '1' means address is within the range of adr_1
 
`ifdef  wb_32bit
 
`ifdef  wb_32bit
 
assign  adr_check_1=i_wb_adr[adr_wb_nb-1:6]==0;
 
assign  adr_check_1=i_wb_adr[adr_wb_nb-1:6]==0;
 
`endif
 
`endif
 
`ifdef  wb_16bit
 
`ifdef  wb_16bit
 
assign  adr_check_1=i_wb_adr[adr_wb_nb-1:5]==0;
 
assign  adr_check_1=i_wb_adr[adr_wb_nb-1:5]==0;
 
`endif
 
`endif
 
`ifdef  wb_64bit
 
`ifdef  wb_64bit
 
assign  adr_check_1=i_wb_adr[adr_wb_nb-1:7]==0;
 
assign  adr_check_1=i_wb_adr[adr_wb_nb-1:7]==0;
 
`endif
 
`endif
 
 
 
 
 
wire    adr_check;      //check address's correctness
 
wire    adr_check;      // A '1' means address is within the range of adr
 
`ifdef wb_16bit
 
`ifdef wb_16bit
 
assign  adr_check=i_wb_adr[4]==0&&adr_check_1;
 
assign  adr_check=i_wb_adr[4]==0&&adr_check_1;
 
`endif
 
`endif
 
`ifdef wb_32bit
 
`ifdef wb_32bit
 
assign  adr_check=i_wb_adr[5]==0&&adr_check_1;
 
assign  adr_check=i_wb_adr[5]==0&&adr_check_1;
Line 131... Line 136... 
`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
 
 
 
reg     RS;
 
always@(posedge i_clk or posedge i_rst)
 
always@(posedge i_clk or posedge i_rst)
 
        if(i_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;
 
                sp<=0;
 
                sp<=0;
 
                pv<=0;
 
                pv<=0;
 
 
 
                RS<=0;
 
        end
 
        end
 
        else    begin
 
        else    begin
 
                if(wack&&(!i_wb_stb)) wack<=0;
 
                if(wack&&(!i_wb_stb)) wack<=0;
 
 
 
                if(RS)RS<=0;
 
                case(state_0)
 
                case(state_0)
 
                0:       begin
 
                0:       begin
 
                        if(we&&(!wack)) state_0<=1;
 
                        if(we&&(!wack)) state_0<=1;
 
                end
 
                end
 
                1:      begin
 
                1:      begin
Line 173... Line 180... 
                                        end
 
                                        end
 
                                        endcase
 
                                        endcase
 
 
 
 
 
                                end
 
                                end
 
                        end
 
                        end
 
 
 
                        else if((adr_1==RS_adr)&&(!wlRS)&&(i_wb_data==0))begin
 
 
 
                                wack<=1;
 
 
 
                                state_0<=0;
 
 
 
                                RS<=1;
 
 
 
                        end
 
                        else begin
 
                        else begin
 
                                 wack<=1;
 
                                 wack<=1;
 
                                state_0<=0;
 
                                state_0<=0;
 
                        end
 
                        end
 
                end
 
                end
 
                endcase
 
                endcase
 
        end
 
        end
 
 
 
 
 
 
 
 
 
 //state machine No.2
 
 //state machine No.2
 
reg     [11:0]state_1;
 
reg     [9:0]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;  // read lock
 
reg     rla;    // read locks
 
reg     rlerr_0;        // read lock
 
reg     rlb;
 
reg     rlerr_1;        // read lock
 
 
 
reg     rla;    // read lock
 
 
 
reg     rlsigma;        // read lock
 
 
 
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;
 
 
 
 
 
reg     [31:0]p;
 
reg     [31:0]p;
 
reg     [31:0]a,sigma,un;
 
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     start;  //start signal for multiplier
 
 
 
 
 
reg     [1:0]mr_index;
 
reg     [1:0]mr_index;
 
reg     md_index;
 
reg     [1:0]md_index;
 
assign  mr=     mr_index==1?kpd:
 
assign  mr=     mr_index==1?kpd:
 
                mr_index==2?kd:ki;
 
                mr_index==2?kd:ki;
 
assign  md=     md_index?err[1]:err[0];
 
assign  md=     md_index==2?err[1]:
 
 
 
                md_index==1?err[0]:sum[15:0];
 
 
 
 
 
reg     cout;
 
 
 
wire    cin;
 
 
 
wire    [31:0]sum;
 
 
 
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)
 
always@(posedge i_clk or posedge i_rst)
 
        if(i_rst)begin
 
        if(i_rst)begin
 
                state_1<=12'b000000000001;
 
                state_1<=12'b000000000001;
 
                wlkp<=0;
 
                wla<=0;
 
                wlki<=0;
 
                wlb<=0;
 
                wlkd<=0;
 
 
 
                wlsp<=0;
 
 
 
                wlpv<=0;
 
 
 
                rlkpd<=0;
 
 
 
                rlerr_0<=0;
 
 
 
                rlerr_1<=0;
 
 
 
                rla<=0;
 
                rla<=0;
 
                rlsigma<=0;
 
                rlb<=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;
Line 253... Line 257... 
                md_index<=0;
 
                md_index<=0;
 
                cout<=0;
 
                cout<=0;
 
        end
 
        end
 
        else begin
 
        else begin
 
                case(state_1)
 
                case(state_1)
 
                12'b000000000001:       begin
 
                10'b0000000001:  begin
 
                        if(update_kpd)begin
 
                        if(update_kpd)begin
 
                                state_1<=12'b000000000010;
 
                                state_1<=10'b0000000010;
 
                                wlkp<=1;
 
                                wla<=1;
 
                                wlkd<=1;
 
                                rla<=1;
 
                                wlpv<=1;
 
 
 
                                rlkpd<=1;
 
 
 
                                rlof<=1;
 
 
 
                        end
 
                        end
 
                        else if(update_esu)begin
 
                        else if(update_esu)begin
 
                                state_1<=12'b000000001000;
 
                                state_1<=10'b0000001000;
 
                                wlkp<=1;
 
                                wla<=1;
 
                                wlki<=1;
 
                                wlb<=1;
 
                                wlkd<=1;
 
                                rlb<=1;
 
                                wlsp<=1;
 
                        end
 
                                wlpv<=1;
 
                        else if(RS)begin        //start a new sequance of U(n)
 
                                rlkpd<=1;
 
                                un<=0;
 
                                rlerr_0<=1;
 
                                sigma<=0;
 
                                rlerr_1<=1;
 
                                of<=0;
 
                                rla<=1;
 
                                err[0]<=0;
 
                                rlsigma<=1;
 
 
 
                                rlof<=1;
 
 
 
                        end
 
                        end
 
                end
 
                end
 
                12'b000000000010:       begin
 
                10'b0000000010:  begin
 
                        p<={{16{kp[15]}},kp};
 
                        p<={{16{kp[15]}},kp};
 
                        a<={{16{kd[15]}},kd};
 
                        a<={{16{kd[15]}},kd};
 
                        state_1<=12'b000000000100;
 
                        state_1<=10'b0000000100;
 
                end
 
                end
 
                12'b000000000100:       begin
 
                10'b0000000100:  begin
 
                        kpd<=sum[15:0];
 
                        kpd<=sum[15:0];
 
                        wlkp<=0;
 
                        wla<=0;
 
                        wlkd<=0;
 
                        rla<=0;
 
                        wlpv<=0;
 
 
 
                        rlkpd<=0;
 
 
 
                        rlof<=0;
 
 
 
                        of[0]<=of_addition[0];
 
                        of[0]<=of_addition[0];
 
                        state_1<=12'b000000000001;
 
                        state_1<=10'b0000000001;
 
                end
 
 
 
                12'b000000001000:       begin
 
 
 
                        p<={{16{~err[0][15]}},~err[0]};
 
 
 
                        a<={31'b0,1'b1};
 
 
 
                        state_1<=12'b000000010000;
 
 
 
                end
 
                end
 
                12'b000000010000:       begin
 
                10'b0000001000:  begin
 
                        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;
 
                        start<=1;                       // start calculate err0 * ki
 
 
 
                        state_1<=10'b0000010000;
 
                end
 
                end
 
                12'b000000100000:       begin
 
                10'b0000010000:  begin
 
                        err[0]<=sum[15:0];
 
                        err[0]<=sum[15:0];
 
                        of[1]<=of_addition[0];
 
                        of[1]<=of_addition[0];
 
 
 
                        of[2]<=of[1];
 
 
 
                        p<={{16{~err[0][15]}},~err[0]};
 
 
 
                        a<={31'b0,1'b1};
 
                        cout<=0;
 
                        cout<=0;
 
                        start<=1;
 
                        mr_index<=1;            // start calculate err0 * kpd   
                        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;
 
                        md_index<=1;
 
                        state_1<=12'b000100000000;
 
                        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
 
                end
 
                12'b000100000000:       begin
 
                10'b0001000000:  begin
 
                        mr_index<=0;
 
                        mr_index<=0;
 
                        md_index<=0;
 
                        md_index<=0;
 
                        start<=0;
 
                        start<=0;
 
                        p<=product;
 
                        p<=product;     // start calculate err0*ki + sigma_last
 
                        a<=sigma;
 
                        a<=sigma;
 
                        state_1<=12'b001000000000;
 
                        state_1<=10'b0010000000;
 
                end
 
                end
 
                12'b001000000000:       begin
 
                10'b0010000000:  begin
 
                        a<=sum;
 
                        a<=sum;         // start calculate err0*kpd + sigma_recent
 
                        sigma<=sum;
 
                        sigma<=sum;
 
                        of[3]<=of[4]|of_addition[1];
 
                        of[3]<=of[4]|of_addition[1];
 
                        of[4]<=of[4]|of_addition[1];
 
                        of[4]<=of[4]|of_addition[1];
 
                        p<=product;
 
                        p<=product;
 
                        state_1<=12'b010000000000;
 
                        state_1<=10'b0100000000;
 
 
 
 
 
                end
 
                end
 
                12'b010000000000:       begin
 
                10'b0100000000:  begin
 
                        a<=sum;
 
                        a<=sum;         // start calculate err0*kpd + sigma_recent+err1*kd
 
                        of[3]<=of[3]|of_addition[1];
 
                        of[3]<=of[3]|of_addition[1];
 
                        p<=product;
 
                        p<=product;
 
                        state_1<=12'b100000000000;
 
                        state_1<=10'b1000000000;
 
                end
 
                end
 
                12'b100000000000:       begin
 
                10'b1000000000:  begin
 
                        un<=sum;
 
                        un<=sum;
 
                        of[3]<=of[3]|of_addition[1];
 
                        of[3]<=of[3]|of_addition[1];
 
                        state_1<=12'b000000000001;
 
                        state_1<=10'b0000000001;
 
                        wlkp<=0;
 
                        wla<=0;
 
                        wlki<=0;
 
                        wlb<=0;
 
                        wlkd<=0;
 
                        rlb<=0;
 
                        wlsp<=0;
 
 
 
                        wlpv<=0;
 
 
 
                        rlkpd<=0;
 
 
 
                        rlerr_0<=0;
 
 
 
                        rlerr_1<=0;
 
 
 
                        rla<=0;
 
 
 
                        rlsigma<=0;
 
 
 
                        rlof<=0;
 
 
 
                end
 
                end
 
                endcase
 
                endcase
 
        end
 
        end
 
 
 
 
 
 
 
 
Line 435... Line 417... 
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,rla,{4{rlb}},rla|rlb,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;
 
 
 
 
 
assign  o_wb_data=adr_check_1?rdata[adr_1]:0;
 
assign  o_wb_data=adr_check_1?rdata[adr_1]:0;
 
assign  o_un=un;
 
assign  o_un=un;
 
assign  o_valid=~rla;
 
assign  o_valid=~rlb;
 
 
 
 
 
 
 
 
 
endmodule
 
endmodule
 
 No newline at end of file
 
 No newline at end of file

powered by: WebSVN 2.1.0

© copyright 1999-2025 OpenCores.org, equivalent to Oliscience, all rights reserved. OpenCores®, registered trademark.