Subversion Repositories pid_controller

/*PID controller testbench

Author: Zhu Xu

Email: m99a1@yahoo.cn

*/

module  PID_tb;

reg     clk=0;

reg     rst=1;

always #1 clk=~clk;

initial begin

        #10 rst=0;

end

//scoreboard

reg     [15:0]kp=0;

reg     [15:0]ki=0;

reg     [15:0]kd=0;

reg     [15:0]sp=0;

reg     [15:0]pv=0;

reg     [15:0]kpd;

reg     [15:0]err[0:1];

reg     [31:0]sigma=0;

reg     [31:0]un=0;

reg     [31:0]a=0;

reg     [31:0]p=0;

reg     [4:0]of=0;

reg     [31:0]s=0;

initial begin

        err[0]=0;

        err[1]=0;

end

wire    [31:0]value_sb[0:10];

assign  value_sb[0]={{16{kp[15]}},kp};

assign  value_sb[1]={{16{ki[15]}},ki};

assign  value_sb[2]={{16{kd[15]}},kd};

assign  value_sb[3]={{16{sp[15]}},sp};

assign  value_sb[4]={{16{pv[15]}},pv};

assign  value_sb[5]={{16{kpd[15]}},kpd};

assign  value_sb[6]={{16{err[0][15]}},err[0]};

assign  value_sb[7]={{16{err[1][15]}},err[1]};

assign  value_sb[8]=un;

assign  value_sb[9]=sigma;

assign  value_sb[10]={27'b0,of};

function of_check_16bit;

        input   [15:0]a,b;

        begin

                s=a+b;

                of_check_16bit=(a[15]&b[15]&(~s[15]))|((~a[15])&(~b[15])&s[15]);

        end

endfunction

function of_check_32bit;

        input   [31:0]a,b;

        begin

                s=a+b;

                of_check_32bit=(a[31]&b[31]&(~s[31]))|((~a[31])&(~b[31])&s[31]);

        end

endfunction

task    update_sb;

        input   [15:0]adr;

        input   [15:0]data;

        begin

                case(adr[15:2])

                        0:begin

                                kp=data;

                                kpd=kp+kd;

                                of[0]=of_check_16bit(kp,kd);

                        end

                        1:begin

                                ki=data;

                        end

                        2:begin

                                kd=data;

                                kpd=kp+kd;

                                of[0]=of_check_16bit(kp,kd);

                        end

                        3:begin

                                sp=data;

                        end

                        4:begin

                                pv=data;

                                err[1]=(~err[0])+1;

                                of[2]=of[1];

                                err[0]=sp+(~pv)+1;

                                of[1]=of_check_16bit(sp,((~pv)+1));

                                p={{16{err[0][15]}},err[0]}*{{16{ki[15]}},ki};

                                of[4]=of[4]|of_check_32bit(sigma,p);

                                sigma=sigma+p;

                                p={{16{err[0][15]}},err[0]}*{{16{kpd[15]}},kpd};

                                of[3]=of[4]|of_check_32bit(sigma,p);

                                a=sigma+p;

                                p={{16{err[1][15]}},err[1]}*{{16{kd[15]}},kd};

                                of[3]=of[3]|of_check_32bit(a,p);

                                un=a+p;

                        end

                endcase

        end

endtask

//wishbone master

wire    [3:0]TAG_s2m;

wire    [3:0]TAG_m2s;

wire    ACK_s2m;

wire    [31:0]ADR_m2s;

wire    CYC_m2s;

wire    [31:0]DAT_s2m;

wire    [31:0]DAT_m2s;

wire    ERR_s2m,RTY_s2m,STB_m2s,WE_m2s;

wire    [3:0]SEL_m2s;

wb_master       wb_master_0(

clk,

rst,

TAG_s2m,

TAG_m2s,

ACK_s2m,

ADR_m2s,

CYC_m2s,

DAT_s2m,

DAT_m2s,

ERR_s2m,

RTY_s2m,

SEL_m2s,

STB_m2s,

WE_m2s

);

reg     [31:0]rdata;

task    check_sb;

        input   [31:0]adr;

        begin

                wb_master_0.rd(adr,rdata);

                if(adr<=10*4)begin

                        if(rdata==value_sb[adr[15:2]])begin

                                $display("%8dns read correct value from address=%8h     data=%8h",$time,adr,rdata);

                        end

                        else begin

                                $display("%8dns read incorrect value from address=%8h   rdata=%8h       scoreboard=%8h",$time,adr,rdata,value_sb[adr[15:2]]);

                        end

                end

                else begin

                        if(rdata==0)begin

                                $display("%8dns read correct value from address=%8h     data=%8h",$time,adr,rdata);

                        end

                        else begin

                                $display("%8dns read incorrect value from address=%8h   rdata=%8h       scoreboard=0",$time,adr,rdata);

                        end

                end

        end

endtask

//instantiation of PID

wire    [31:0]o_un;

wire    valid;

PID     PID_0(

clk,

rst,

CYC_m2s,

STB_m2s,

WE_m2s,

ADR_m2s[15:0],

DAT_m2s,

ACK_s2m,

DAT_s2m,

o_un,

valid

);

//test procedure

reg     signed[31:0]rdata_1=0;

initial begin

        while(rst)begin

                @(posedge clk);

        end

        wb_master_0.wr(0*4,32'h80,4'b1111);

        update_sb(0*4,16'h80);

        wb_master_0.wr(1*4,32'h5,4'b1111);

        update_sb(1*4,16'h5);

        wb_master_0.wr(2*4,32'h5,4'b1111);

        update_sb(2*4,16'h5);

        wb_master_0.wr(3*4,32'hf87,4'b1111);

        update_sb(3*4,16'hf87);

        wb_master_0.wr(4*4,32'h0,4'b1111);

        update_sb(4*4,0);

        repeat(1000)begin

                wb_master_0.rd(8*4,rdata_1);

                check_sb(8*4);

                rdata_1=rdata_1>>>8;

                wb_master_0.wr(4*4,rdata_1,4'b1111);

                update_sb(4*4,rdata_1[15:0]);

        end

        #10 $finish;

end

endmodule