voapidtune.v

FPGA verilog

module VOAPIDTune(
	EN,
	CLK,
	Ramp,
	Ki,
	Kp,
	NextPoint,
	Target,
	PIDResult
);
input			EN;
input			CLK;
input			Ramp;
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

reg		[31:0]	TargetReg;

wire			Update;
reg		[9:0]	Counter;


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;

wire			OverHi,OverLo;
wire			IOver;

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

lpm_sub_signed_32	b2v_inst(.dataa({7'b00000,TargetReg[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(Result));

assign Update = Counter[9]==1'b1;

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 (Update)
		begin
			if (TargetReg[31:7]<Target[31:7])
				TargetReg <= TargetReg + 8'b10000000;
			else if (TargetReg[31:7]>Target[31:7])
				TargetReg <= TargetReg - 8'b10000000;
			else
				TargetReg <= Target;
		end
	end
end

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

always @(posedge CLK)
begin
	if (EN)
	begin
		if (Update)
		begin
			if (TargetReg>327)//0.01
			begin
				if (OverLo)
					PIDResult <= 12'h000;
				else if (OverHi)
					PIDResult <= 12'hFFF;
				else
					PIDResult <= Result[19:8];
			end
			else
				PIDResult <= 12'h000;
			Counter <= 10'd0;
		end
		else
			Counter <= Counter + 1'b1;
	end
end
endmodule