data/full_repos/permissive/159830616/YJ431_PL.srcs/sources_1/new/FB_3poPID.v
'utf-8' codec can't decode byte 0xb7 in position 1507: invalid start byte
1: b'%Warning-MULTITOP: data/full_repos/permissive/159830616/YJ431_PL.srcs/sources_1/new/FB_3poPID.v:520: Multiple top level modules\n                                                                                                   : ... Suggest see manual; fix the duplicates, or use --top-module to select top.\n                   ... Use "/* verilator lint_off MULTITOP */" and lint_on around source to disable this message.\n                                                                                                   : ... Top module \'FB_po3PIDREG\'\nmodule FB_po3PIDREG(\n       ^~~~~~~~~~~~\n                                                                                                   : ... Top module \'po3PID\'\nmodule po3PID(\n       ^~~~~~\n%Error: Exiting due to 1 warning(s)\n'
module po3PID(

	CLK,
	RST_n,


	CTL_FREQ_Set,
	PID_AIM_Set,
	PID_CUR_Set,

	PID_ERS_Set,
	PID_KPS_Set,

	PID_KDS_Set,

	PID_ERM_Set,
	PID_KPM_Set,

	PID_KDM_Set,

	PID_ERB_Set,
	PID_KPB_Set,

	PID_KDB_Set,

	PID_OUT_REG

	);

input CLK;
input RST_n;

input [31:0] CTL_FREQ_Set;
input [31:0] PID_AIM_Set;
input [31:0] PID_CUR_Set;

input [31:0] PID_ERS_Set;
input [31:0] PID_KPS_Set;

input [31:0] PID_KDS_Set;

input [31:0] PID_ERM_Set;
input [31:0] PID_KPM_Set;

input [31:0] PID_KDM_Set;

input [31:0] PID_ERB_Set;
input [31:0] PID_KPB_Set;

input [31:0] PID_KDB_Set;

output reg [31:0] PID_OUT_REG;



reg [31:0] CTL_FREQ_REG = 32'd1000000000;
reg signed [31:0] PID_AIM_REG = 32'd0;
reg signed [31:0] PID_CUR_REG = 32'd0;
reg signed [31:0] PID_ERS_REG = 32'd0;
reg signed [31:0] PID_KPS_REG = 32'd0;

reg signed [31:0] PID_KDS_REG = 32'd0;
reg signed [31:0] PID_ERM_REG = 32'd0;
reg signed [31:0] PID_KPM_REG = 32'd0;

reg signed [31:0] PID_KDM_REG = 32'd0;
reg signed [31:0] PID_ERB_REG = 32'd0;
reg signed [31:0] PID_KPB_REG = 32'd0;

reg signed [31:0] PID_KDB_REG = 32'd0;


always @(negedge CLK or negedge RST_n)begin
	if ( !RST_n )begin
		CTL_FREQ_REG <= 32'd1000000000;
		PID_AIM_REG <= 32'd0;
		PID_CUR_REG <= 32'd0;
		PID_ERS_REG <= 32'd0;
		PID_KPS_REG <= 32'd0; 

		PID_KDS_REG <= 32'd0;
		PID_ERM_REG <= 32'd0;
		PID_KPM_REG <= 32'd0;

		PID_KDM_REG <= 32'd0;
		PID_ERB_REG <= 32'd0;
		PID_KPB_REG <= 32'd0;

		PID_KDB_REG <= 32'd0;
	end
	else begin
		CTL_FREQ_REG <= CTL_FREQ_Set;
		
		PID_AIM_REG <= PID_AIM_Set;
		PID_CUR_REG <= PID_CUR_Set;
		
		PID_ERS_REG <= PID_ERS_Set;
		PID_KPS_REG <= PID_KPS_Set; 

		PID_KDS_REG <= PID_KDS_Set;
		
		PID_ERM_REG <= PID_ERM_Set;
		PID_KPM_REG <= PID_KPM_Set;

		PID_KDM_REG <= PID_KDM_Set;
		
		PID_ERB_REG <= PID_ERB_Set;
		PID_KPB_REG <= PID_KPB_Set;

		PID_KDB_REG <= PID_KDB_Set;
	end 
end



reg [31:0] ctl_cnt = 32'd0;
reg signed [31:0] ERROR[2:0];

reg signed [31:0] KP_CAL32_REG = 32'd0;
reg signed [31:0] KD_CAL32_REG = 32'd0;
reg signed [31:0] KD_DIF32_REG = 32'd0;
reg signed [31:0] PID_NERS_REG = 32'd0;
reg signed [31:0] PID_NERM_REG = 32'd0;
reg signed [31:0] PID_NERB_REG = 32'd0;

reg clk_pre = 1'b0;

always@ ( posedge CLK or negedge RST_n ) begin
    if ( !RST_n ) begin
        clk_pre <= 1'b0;
    end
    else begin
        clk_pre <= ~clk_pre;
    end  
end

always@ ( posedge clk_pre or negedge RST_n )begin
	if ( !RST_n ) begin 
		ctl_cnt <= 32'd0;
        ERROR[0] <= 32'd0;
        ERROR[1] <= 32'd0;
        ERROR[2] <= 32'd0;

        PID_NERS_REG <= 32'd0;
        PID_NERM_REG <= 32'd0;
        PID_NERB_REG <= 32'd0;
	end

	else begin		
		if ( ctl_cnt == 32'd0 ) begin
			ctl_cnt <= ctl_cnt + 32'd1;
	       
            ERROR[0][31:0] <= PID_CUR_REG[31:0] - PID_AIM_REG[31:0];
            ERROR[1][31:0] <= ERROR[0][31:0];
            ERROR[2][31:0] <= ERROR[1][31:0];
            
            PID_NERS_REG <= -PID_ERS_REG;
            PID_NERM_REG <= -PID_ERM_REG;
            PID_NERB_REG <= -PID_ERB_REG;
					
		end
		else if ( ctl_cnt == 32'd1 ) begin
			ctl_cnt <= ctl_cnt + 32'd1;
			
			KD_DIF32_REG [31:0] <=  ERROR[0][31:0] - ERROR[1][31:0];

			if (  ERROR[0] > PID_ERB_REG || ERROR[0] < PID_NERB_REG ) begin
			
			     KP_CAL32_REG <= PID_KPB_REG;
			     KD_CAL32_REG <= PID_KDB_REG;
			end

			else if ( ERROR[0] > PID_ERM_REG || ERROR[0] < PID_NERM_REG ) begin
                 KP_CAL32_REG <= PID_KPM_REG;
                 KD_CAL32_REG <= PID_KDM_REG;
			end

			else if ( ERROR[0] > PID_ERS_REG || ERROR[0] < PID_NERS_REG )begin
                 KP_CAL32_REG <= PID_KPS_REG;
                 KD_CAL32_REG <= PID_KDS_REG;
			end
			else begin
                KP_CAL32_REG <= 32'b0;
                KD_CAL32_REG <= 32'b0;
			end

		end
		else if ( ctl_cnt == 32'd2 ) begin
			ctl_cnt <= ctl_cnt + 32'd1;

			PID_OUT_REG <= KP_CAL32_REG * ERROR[0] + KD_CAL32_REG * KD_DIF32_REG;

		end

		else if ( ctl_cnt >= CTL_FREQ_REG )	begin
			ctl_cnt <= 32'd0;
		end
		else begin
			ctl_cnt <= ctl_cnt + 32'd1;
		end

	end
end

endmodule
module po3PID(

	CLK,
	RST_n,


	CTL_FREQ_Set,
	PID_AIM_Set,
	PID_CUR_Set,

	PID_ERS_Set,
	PID_KPS_Set,

	PID_KDS_Set,

	PID_ERM_Set,
	PID_KPM_Set,

	PID_KDM_Set,

	PID_ERB_Set,
	PID_KPB_Set,

	PID_KDB_Set,

	PID_OUT_REG

	);


input CLK;
input RST_n;

input [31:0] CTL_FREQ_Set;
input [31:0] PID_AIM_Set;
input [31:0] PID_CUR_Set;

input [31:0] PID_ERS_Set;
input [31:0] PID_KPS_Set;

input [31:0] PID_KDS_Set;

input [31:0] PID_ERM_Set;
input [31:0] PID_KPM_Set;

input [31:0] PID_KDM_Set;

input [31:0] PID_ERB_Set;
input [31:0] PID_KPB_Set;

input [31:0] PID_KDB_Set;

output reg [31:0] PID_OUT_REG;



reg [31:0] CTL_FREQ_REG = 32'd1000000000;
reg signed [31:0] PID_AIM_REG = 32'd0;
reg signed [31:0] PID_CUR_REG = 32'd0;
reg signed [31:0] PID_ERS_REG = 32'd0;
reg signed [31:0] PID_KPS_REG = 32'd0;

reg signed [31:0] PID_KDS_REG = 32'd0;
reg signed [31:0] PID_ERM_REG = 32'd0;
reg signed [31:0] PID_KPM_REG = 32'd0;

reg signed [31:0] PID_KDM_REG = 32'd0;
reg signed [31:0] PID_ERB_REG = 32'd0;
reg signed [31:0] PID_KPB_REG = 32'd0;

reg signed [31:0] PID_KDB_REG = 32'd0;


always @(negedge CLK or negedge RST_n)begin
	if ( !RST_n )begin
		CTL_FREQ_REG <= 32'd1000000000;
		PID_AIM_REG <= 32'd0;
		PID_CUR_REG <= 32'd0;
		PID_ERS_REG <= 32'd0;
		PID_KPS_REG <= 32'd0; 

		PID_KDS_REG <= 32'd0;
		PID_ERM_REG <= 32'd0;
		PID_KPM_REG <= 32'd0;

		PID_KDM_REG <= 32'd0;
		PID_ERB_REG <= 32'd0;
		PID_KPB_REG <= 32'd0;

		PID_KDB_REG <= 32'd0;
	end
	else begin
		CTL_FREQ_REG <= CTL_FREQ_Set;
		
		PID_AIM_REG <= PID_AIM_Set;
		PID_CUR_REG <= PID_CUR_Set;
		
		PID_ERS_REG <= PID_ERS_Set;
		PID_KPS_REG <= PID_KPS_Set; 

		PID_KDS_REG <= PID_KDS_Set;
		
		PID_ERM_REG <= PID_ERM_Set;
		PID_KPM_REG <= PID_KPM_Set;

		PID_KDM_REG <= PID_KDM_Set;
		
		PID_ERB_REG <= PID_ERB_Set;
		PID_KPB_REG <= PID_KPB_Set;

		PID_KDB_REG <= PID_KDB_Set;
	end 
end



reg [31:0] ctl_cnt = 32'd0;
reg signed [31:0] ERROR[2:0];

reg signed [31:0] KP_CAL32_REG = 32'd0;
reg signed [31:0] KD_CAL32_REG = 32'd0;
reg signed [31:0] KD_DIF32_REG = 32'd0;
reg signed [31:0] PID_NERS_REG = 32'd0;
reg signed [31:0] PID_NERM_REG = 32'd0;
reg signed [31:0] PID_NERB_REG = 32'd0;

reg clk_pre = 1'b0;

always@ ( posedge CLK or negedge RST_n ) begin
    if ( !RST_n ) begin
        clk_pre <= 1'b0;
    end
    else begin
        clk_pre <= ~clk_pre;
    end  
end

always@ ( posedge clk_pre or negedge RST_n )begin
	if ( !RST_n ) begin 
		ctl_cnt <= 32'd0;
        ERROR[0] <= 32'd0;
        ERROR[1] <= 32'd0;
        ERROR[2] <= 32'd0;

        PID_NERS_REG <= 32'd0;
        PID_NERM_REG <= 32'd0;
        PID_NERB_REG <= 32'd0;
	end

	else begin		
		if ( ctl_cnt == 32'd0 ) begin
			ctl_cnt <= ctl_cnt + 32'd1;
	       
            ERROR[0][31:0] <= PID_CUR_REG[31:0] - PID_AIM_REG[31:0];
            ERROR[1][31:0] <= ERROR[0][31:0];
            ERROR[2][31:0] <= ERROR[1][31:0];
            
            PID_NERS_REG <= -PID_ERS_REG;
            PID_NERM_REG <= -PID_ERM_REG;
            PID_NERB_REG <= -PID_ERB_REG;
					
		end
		else if ( ctl_cnt == 32'd1 ) begin
			ctl_cnt <= ctl_cnt + 32'd1;
			
			KD_DIF32_REG [31:0] <=  ERROR[0][31:0] - ERROR[1][31:0];

			if (  ERROR[0] > PID_ERB_REG || ERROR[0] < PID_NERB_REG ) begin
			
			     KP_CAL32_REG <= PID_KPB_REG;
			     KD_CAL32_REG <= PID_KDB_REG;
			end

			else if ( ERROR[0] > PID_ERM_REG || ERROR[0] < PID_NERM_REG ) begin
                 KP_CAL32_REG <= PID_KPM_REG;
                 KD_CAL32_REG <= PID_KDM_REG;
			end

			else if ( ERROR[0] > PID_ERS_REG || ERROR[0] < PID_NERS_REG )begin
                 KP_CAL32_REG <= PID_KPS_REG;
                 KD_CAL32_REG <= PID_KDS_REG;
			end
			else begin
                KP_CAL32_REG <= 32'b0;
                KD_CAL32_REG <= 32'b0;
			end

		end
		else if ( ctl_cnt == 32'd2 ) begin
			ctl_cnt <= ctl_cnt + 32'd1;

			PID_OUT_REG <= KP_CAL32_REG * ERROR[0] + KD_CAL32_REG * KD_DIF32_REG;

		end

		else if ( ctl_cnt >= CTL_FREQ_REG )	begin
			ctl_cnt <= 32'd0;
		end
		else begin
			ctl_cnt <= ctl_cnt + 32'd1;
		end

	end
end

endmodule