pidtunebak.v

FPGA verilog

module PIDTune(
	EN,
	CLK,
	FeedForwardEN,
	FeedForwardData,
	Ki,
	Kp,
	NextPoint,
	Target,
	PIDResult
);
input			EN;
input			CLK;
input			FeedForwardEN;
input	[11:0]	FeedForwardData;
input	[31:0] 	Ki;
input	[31:0] 	Kp;
input	[31:0] 	NextPoint;
input	[31:0] 	Target;
output	[11:0] 	PIDResult;

reg		[11:0] 	PIDResult;

reg		[31:0] 	DacData;

reg		[31:0] Err_K_1;

reg		[31:0]	Ki0,Kp0,Target0;
reg		[31:0]	KiSet,KpSet,TargetSet;

wire	[31:0] Err_K__Err_K_1;
wire	[48:0] Err_P;
wire	[48:0] Err_I;

wire 	[31:0] Error;

wire	[31:0]	Result,Result0,Result1;

reg		[1:0]	Counter;

lpm_sub0	b2v_inst(.dataa(TargetSet),
					.datab(NextPoint),
					.result(Error));

lpm_mult1	b2v_inst2(.dataa(Error),
					.datab(KiSet),
					.result(Err_I));

lpm_sub0	b2v_inst4(.dataa(Error),
					.datab(Err_K_1),
					.result(Err_K__Err_K_1));

lpm_mult1	b2v_inst5(.dataa(Err_K__Err_K_1),
					.datab(KpSet),
					.result(Err_P));

lpm_add0	b2v_inst1(.dataa(Err_I[31:0]),
					.datab(Err_P[31:0]),
					.result(Result0));

lpm_add0	b2v_add3(.dataa(Result0),
					.datab({5'h00,FeedForwardData,15'h0000}),
					.result(Result1));


always @(posedge CLK)
begin
	Ki0 <= Ki;
	Kp0 <= Kp;
	Target0 <= Target;
end					

always @(posedge CLK)
begin
	KiSet <= Ki0;
	KpSet <= Kp0;
	TargetSet <= Target0;
end	

always @(posedge CLK)
begin
	if (EN)
		Err_K_1 <= Error;
	else
		Err_K_1 <= 0;
end

assign Result = FeedForwardEN?Result1:Result0;

always @(posedge CLK)
begin
	if (EN)
	begin
		if (Result[31]==1)
			PIDResult <= 12'h000;
		else if (Result[30:27]!=4'b0000)
			PIDResult <= 12'hFFF;
		else
			PIDResult <= Result[26:15];
	end
end

endmodule