pidtunebak4.v

FPGA verilog

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

reg		[11:0] 	PIDResult;

reg		[31:0] SumError;

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

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

reg		[31:0]	LastError;

wire 	[31:0] 	Error,Error_Single;

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

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

lpm_add0	b2v_inst6(.dataa(SumError), // 当前积分
					.datab(Error),
					.result(Error_Single));

lpm_sub0	b2v_inst4(.dataa(LastError),  // 当前微分
					.datab(Error),
					.result(Err_K__Err_K_1));

lpm_mult1	b2v_inst2(.dataa(Error_Single),  // 积分项
					.datab(KiSet),
					.result(Err_I));

lpm_mult1	b2v_inst5(.dataa(Error),// 比例项
					.datab(KpSet),
					.result(Err_P));

lpm_mult1	b2v_inst7(.dataa(Err_K__Err_K_1),// 微分项
					.datab(Kd),
					.result(Err_D));

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

lpm_add0	b2v_inst8(.dataa(Result0),
					.datab(Err_D[31:0]),
					.result(Result1));

lpm_add0	b2v_add3(.dataa(Result1),
					.datab(FeedForwardData),
					.result(Result2));

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)
	begin
		SumError <= Error_Single;
		LastError <= Error;
	end
	else begin
		SumError <= 0;
		LastError <= 0;
	end
end

assign Result = FeedForwardEN?Result2:Result1;

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