pidtune.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] SumError; //Q8

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

wire	[31:0]	I_Err;

wire 	[31:0] 	Error,Error_Single;

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

wire			OverHi,OverLo;
wire			IOver;

wire	[31:0]	AbsResult0,AbsResult1,Tmp;

lpm_sub_signed_32	b2v_inst(.dataa({7'b00000,Target[31:7]}),//Q8
					.datab({7'b00000,NextPoint[31:7]}),//Q8
					.result(Error));

lpm_add_signed_32	b2v_inst6(.dataa(SumError),
					.datab(Error),
					.result(Error_Single));
					
lpm_mult_signed_32x32_hard	b2v_inst2(.dataa(I_Err),
					.datab(Ki),
					.result(Err_I));

lpm_mult_signed_32x32_hard	b2v_inst5(.dataa(Error),
					.datab(Kp),
					.result(Err_P));

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

lpm_add_signed_32	b2v_add3(.dataa(Result0),
					.datab({12'h0000,FeedForwardData,8'h00}),
					.result(Result1));

assign Result = FeedForwardEN?Result1:Result0;


assign OverLo = Result[31]==1;
assign OverHi = Result[30:20]!=11'b00000000000;


lpm_abs_32 ErrorAbs0(.data(Error),.result(AbsResult0));

lpm_abs_32 ErrorAbs1(.data(SumError),.result(AbsResult1));

lpm_sub_signed_32	b2v_inst8(.dataa(32'd2147483647),
					.datab(AbsResult1),
					.result(Tmp));
					
assign IOver = (Error[31]==SumError[31])&(AbsResult0>Tmp);
assign I_Err = (IOver)?SumError:Error_Single;

always @(posedge CLK)
begin
	if (EN)
	begin
		if (!IOver)
			SumError <= Error_Single;
	end
end

//always @(posedge CLK)
always @(OverLo or OverHi or Result)
begin
	//if (EN)
	begin
		if (OverLo)
			PIDResult <= 12'h028;
		else if (OverHi)
			PIDResult <= 12'hFFF;
		else
			PIDResult <= Result[19:8];
	end
end

endmodule