PID Controller

_see_it.md

Hardware Demo of a Digital PID Controller - YouTube

PID.rb

class PID
  attr_reader :kp, :ki, :kd, :output, :previous_error
  attr_accessor :setpoint, :history_depth, :output_range, :invert_output

  def initialize(setpoint, options = {})
    @setpoint = setpoint
    @kp = options[:kp] || 1
    @ki = options[:ki] || 0
    @kd = options[:kd] || 0

    @history_depth = options[:history_depth] || 0
    @output_range = options[:output_range] || (-1..1)
    @invert_output = options[:invert_output] || false
    reset
  end
  
  def reset
    @last_time = nil
    @previous_error = 0.0
    @integrative = 0.0
    @history = []
  end

  def control(input)
    dt = step_time
    error = setpoint - input
    
    out = proportional(error) + integrative(error, dt) + derivative(error, dt)
    out = -out if invert_output
    @previous_error = error
    @output = out.clamp_to output_range
  end
  
  protected # -------
  
  def step_time
    now = Time.now.to_f
    dt  = @last_time.nil? ? 1.0 : now - @last_time
    @last_time = now
    dt
  end
  
  def proportional(error)
    kp * error
  end
    
  def integrative(error, dt)
    # classic mode
    @integrative += error * dt
    # window mode
    if @history_depth > 0
      @history << error * dt                    # push last sample
      @history = @history.last(@history_depth)  # keep the last one
      @integrative = @history.reduce(:+)
      @integrative /= @history_depth            # normalize
    end
    ki * @integrative
  end
  
  def derivative(error, dt)
    kd * (error - @previous_error) / dt
  end
end

module Comparable
  def clamp_to(range)
     return range.min if self < range.min
     return range.max if self > range.max
     self
  end
end

pid_tester.rb

require './pid'

class Vessel
  attr_accessor :speed, :engine_force, :grav_force
  
  def initialize
    @grav_force = -150
    @engine_force = 0
    @speed = 0 
  end
  
  def step
    @grav_force -= 0.5 if @grav_force > -500
    @engine_force = 0 if @engine_force < 0
    @speed += @grav_force + @engine_force
    puts "#{grav_force} + #{engine_force} = #{speed}"
  end
end

pid = PID.new(-51.3, kp: 1, kd: 0.1, output_range: -100..100)
v = Vessel.new

File.open "out", "w+" do |f|
  for i in 1..50
    c = pid.control v.speed
    f.puts [v.speed.to_f.round(3), c.to_f.round(3), pid.previous_error.to_f.round(3)].join(", ")
    v.engine_force += c
    pid.setpoint = -200 if i == 30
    v.step
  end
end

puts v.grav_force
puts v.engine_force
puts v.speed