module Ros.Util.PID where
import Data.IORef (newIORef, readIORef, writeIORef)
import Data.Time.Clock (getCurrentTime, diffUTCTime)
pidUniform :: Fractional a => a -> a -> a -> a -> a -> a -> a -> (a, a)
pidUniform kp ki kd obj = pidFixed kp ki kd obj 1
pidFixed :: Fractional a => a -> a -> a -> a -> a -> a -> a -> a -> (a,a)
pidFixed kp ki kd obj dt e1 e2 x = (e3, output)
where e3 = x obj
invDt = 1 / dt
scale = dt / 3
integral = scale * (e1 + 4 * e2 + e3)
derivative = (e3 e2) * invDt
output = kp * e3 + ki * integral + kd * derivative
pidTimed :: Fractional a => a -> a -> a -> a -> (a,a) -> (a,a) -> (a,a) -> (a,a)
pidTimed kp ki kd obj (t1,e1) (_,e2) (t3,x) = (e3, output)
where e3 = x obj
scale = (t3 t1) / 6
integral = scale * (e1 + 4 * e2 + e3)
derivative = e3 e2
output = kp * e3 + ki * integral + kd * derivative
pidFixedIO :: Fractional a => a -> a -> a -> a -> IO (a -> a -> IO a)
pidFixedIO kp ki kd dt =
do e1 <- newIORef 0
e2 <- newIORef 0
initialized <- newIORef (0::Int)
return $ \setpoint ->
let pid' = pidFixed kp ki kd setpoint dt
in \x ->
do init' <- readIORef initialized
case init' of
0 -> do writeIORef e1 (x setpoint)
writeIORef initialized 1
return 0
1 -> do writeIORef e2 (x setpoint)
writeIORef initialized 2
return 0
_ -> do e1' <- readIORef e1
e2' <- readIORef e2
let (e3,c) = pid' e1' e2' x
writeIORef e1 e2'
e3 `seq` writeIORef e2 e3
return c
pidUniformIO :: Fractional a => a -> a -> a -> IO (a -> a -> IO a)
pidUniformIO kp ki kd = pidFixedIO kp ki kd 1
pidTimedIO :: Fractional a => a -> a -> a -> IO (a -> a -> IO a)
pidTimedIO kp ki kd =
do go <- pidWithTimeIO kp ki kd
start <- getCurrentTime
return $ \setpoint -> \x ->
do t <- fmap (realToFrac . flip diffUTCTime start) getCurrentTime
go setpoint (t,x)
pidWithTimeIO :: Fractional a => a -> a -> a -> IO (a -> (a,a) -> IO a)
pidWithTimeIO kp ki kd =
do e1 <- newIORef undefined
e2 <- newIORef undefined
initialized <- newIORef (0::Int)
return $ \setpoint ->
let pid' = pidTimed kp ki kd setpoint
in \(t,x) ->
do init' <- readIORef initialized
case init' of
0 -> do writeIORef e1 (t, x setpoint)
writeIORef initialized 1
return 0
1 -> do writeIORef e2 (t, x setpoint)
writeIORef initialized 2
return 0
_ -> do e1' <- readIORef e1
e2' <- readIORef e2
let (e3,c) = pid' e1' e2' (t,x)
writeIORef e1 e2'
e3 `seq` writeIORef e2 (t,e3)
return c