A simple, one dimensional PID controller written in Python

pid_controller.py

import robot # dummy class


class PidController:
    """Implements a one-dimensional PID controller that also takes care of
    integral windup in case of large changes in the setpoint.
    """
    def __init__(self, gains=None, max_windup=None):
        """Initializes the PidController object
        Arguments:
            gains (tuple of floats): the gains of the proportional, integral,
                and derivative portions of the controller, respectively
            max_windup (float): the maximum (positive and negative) amount
                allowed for the integrator
        """
        if gains is not None:
            self.kp, self.ki, self.kd = gains
        else:
            self.kp, self.ki, self.kd = 1., 1., 1.

        if max_windup is not None:
            self.max_windup = max_windup
        else:
            self.max_windup = 1e6

        self.total_error = 0.0
        self.previous_error = 0.0

    def update(self, measured_value, setpoint, dt=1.0):
        """Updates the PID controller
        Arguments:
            measured_value (float): the measured value
            setpoint (float): the reference point
        Returns:
            The value (float) to move the system to the setpoint
        """
        error =  setpoint - measured_value
        self.total_error += error
        differential_error = error - self.previous_error
        self.previous_error = error

        # constrain the integral windup because there might be some undesirable
        # behavior otherwise
        self.total_error = min([self.total_error, self.max_windup])
        self.total_error = max([self.total_error, -self.max_windup])

        p_term = self.kp * error
        i_term = self.ki * self.total_error * dt
        d_term = self.kd * differential_error / dt

        return p_term + i_term + d_term

    def reset(self):
        """Sets the total error and previous error to 0.0"""
        self.total_error = 0.0
        self.previous_error = 0.0
        

# this will execute if ran as `python pid_controller.py`
if __name__ == '__main__':
    # instantiate robot at state (x, y, theta) = (0, 0, 0)
    my_robot = robot()
    
    gains = 16.0, 0.5, 1.0
    max_windup = 0.8
    pid = PidController(gains, max_windup)
    
    x_position = 10.0   # meters
    tolerance = 0.1     # meters
    
    # continue driving along x-axis until we get within tolerance meters of goal
    while abs(robot.x - x_position) > tolerance:
        # pid.update(current_position, goal_position)
        distance = pid.update(my_robot.x, x_position)
        
        print(f'Distance to goal is {distance}')
        
        # my_robot.move(angle, distance)
        my_robot.move(0.0, distance)