module System.Hardware.Acquisition.Adaptor.PID
( Parameters (..)
, adaptor
) where
import Prelude
import Control.Monad.Error
import Data.Maybe
import Data.IORef
import Text.Printf
import System.Hardware.Acquisition as Acquisition
data Parameters = Parameters
{ kp :: Value
, ti :: Value
, td :: Value
} deriving (Show)
type State = (Value, Value)
adaptor
:: MonadIO m
=> Channel
-> IORef Parameters
-> Adaptor m
adaptor setPointChan params c = liftIO $ do
state <- newIORef (0.0, 0.0)
let getSample :: (Error e, MonadError e m, MonadIO m) => m Sample
getSample = do
Parameters {..} <- liftIO (readIORef params)
setPoint <- return value `ap` Acquisition.getSample setPointChan
Sample {..} <- Acquisition.getSample c
let proportional = setPoint value
(integral, derivative) <- liftIO $ atomicModifyIORef state $
\ (int, previous) -> let
integral = int + proportional
derivative = value previous in
( (integral, value)
, (integral, derivative) )
let value = kp *
( proportional
+ integral / ti
+ derivative * td )
liftIO (printf "SP: %.1f, PID: %.1f, %.1f, %.1f\n"
setPoint proportional integral derivative)
return Sample {..}
let close :: MonadIO m => m ()
close = return ()
return Channel {..}