pid.py

class pidController:

    def __init__(self, kp, ki, kd, max_integral_error, error_margin):

        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.max_integral_error = max_integral_error
        self.error_margin = error_margin
        self.total_error = 0
        self.prev_error = 0
        self.prev_time = 0
        self.first_measurement = True

    def compute_control(self, x_desired, x_actual, curr_time):

	error = - (x_desired - x_actual)

	# If first measurement disregard control terms based on previous timestep
	if (self.first_measurement):
	    self.first_measurement = False
	    self.prev_error = error
	    self.prev_time = curr_time

	    return 0

	    ## If error is too small, don't compute control
	    #if (abs(error) < self.error_margin):
	    #    return 0

	    #control = kp * error

	    #return control

	# If error is too small, don't compute control
	if (abs(error) < self.error_margin):
	    return 0

	self.total_error = self.total_error + error * (curr_time - self.prev_time)

        # Reset total error when max_integral_error reached
        if self.total_error > self.max_integral_error:
            self.total_error = 0

	error_rate = (error - self.prev_error) * (curr_time - self.prev_time)
	control = self.kp * error + self.ki * self.total_error + self.kd * error_rate


	self.prev_error = error
	self.prev_time = curr_time

        return control