`timescale 1ns / 1ps
//////////////////////////////////////////////////////////////////////////////////
// PID feedback module
// Joseph Coplon
// CPE 439
// Fall 2016
//////////////////////////////////////////////////////////////////////////////////


module pid(
    temp_error,
    clock,
    reset,
    k,
    temp_out);

input signed [7:0] temp_error;
input clock;
input reset;
input [23:0] k;
output signed [7:0] temp_out;

logic signed [7:0] temp_error;
logic clock;
logic reset;
logic [23:0] k;
logic signed [7:0] temp_out;

logic signed [7:0] temp_prev;
logic signed [7:0] temp_error_prev[1:2];
logic [23:0] clockAccumulator;
logic pidClock = 0;

initial begin
    temp_prev = 0;
    temp_error_prev[1] = 0;
    temp_error_prev[2] = 0;
    temp_out = 'd25;
end

// clock divider
// convers default 100 MHz to 20 Hz
always @(posedge clock) begin
    if (clockAccumulator > 'd5000000) begin
        pidClock = ~pidClock;
        clockAccumulator = 0;
    end
    else
        clockAccumulator = clockAccumulator + 1;
end

// update error values
always @(posedge pidClock) begin
	if (reset==1) begin
		temp_prev = 0;
		temp_error_prev[1] = 0;
		temp_error_prev[2] = 0;
	end
	else begin
		temp_error_prev[2] = temp_error_prev[1];
		temp_error_prev[1] = temp_error;
		temp_prev = temp_out;
	end
end

// assign output using PID parameters
always_comb begin
    temp_out = temp_prev + signed'(k[23:16])*temp_error + signed'(k[15:8])*temp_error_prev[1] + signed'(k[7:0])*temp_error_prev[2];
    if (temp_out > 'd90)
        temp_out = 'd90;
    else if (temp_out < 'd25)
        temp_out = 'd25;
end

endmodule