previous_error = 0integral = 0 start: error = setpoint - actual_position integral = integral + (error*dt) derivative = (error - previous_error)/dt output = (Kp*error) + (Ki*integral) + (Kd*derivative) previous_error = error wait(dt) goto start