pid.swift

// Based on pseudocode at https://en.wikipedia.org/wiki/PID_controller#Pseudocode

import Foundation

protocol PIDControllee {
    
    func measured_value() -> Float;
    func output(_ v:Float);
}

class PIDController {
    
    let controllee: PIDControllee
    let Kp: Float
    let Ki: Float
    let Kd: Float
    let dt: TimeInterval

    var done = false

    init(p:Float, i:Float, d:Float, controllee c:PIDControllee, interval:TimeInterval) {
        controllee = c
        Kp = p
        Ki = i
        Kd = d
        dt = interval
    }
    
    func run() {
        var previous_error = 0
        var integral = 0
        repeat {
            let error = setpoint - controllee.measured_value()
            integral = integral + error * dt
            let derivative = (error - previous_error) / dt
            
            out(e:error, i:integral, d:derivative)
            previous_error = error
            wait(dt)
        } while !done
    }
    
    func out(e: Float, i: Float, d: Float) {
        controllee.output(Kp * e + Ki * i + Kd * d)
    }
    
    func wait(_ d:TimeInterval) { // TODO: sleep?
    }
}