pid.hs

module Pid where
import CLaSH.Prelude as C

{-# ANN topEntity
  (defTop
    { t_name     = "pid"
    , t_inputs   = ["initI","initD","kP","kI","kD","setvalue"]
    , t_outputs  = ["pidsignal"]
    , t_extraIn  = [ ("CLOCK", 1)]
    ,  t_clocks  = [ clockWizard "clkwiz50" "CLOCK[0]" "~ KEY[0]" ]
    }) #-}

(+:) = satPlus  SatBound -- operator +: is saturated addition 
(-:) = satMin  SatBound 

sigKP kp = fmap (kp*) 

sigKI init ki = mealyB trans init where
    trans acc e = (out,out) 
      where out = acc +: (ki * e)

sigKD init kd = mealyB trans init where 
    trans preve e = (e , (e -: preve )*kd)

-- PID part (without looback)
pidProc (initI,initD,kP,kI,kD) signal = sumsig <$> bundle (sp,si,sd)
    where 
        sp = sigKP kP signal
        si = sigKI initI kI signal
        sd = sigKD initD kD signal
        sumsig (k,i,d) = k +: i +: d
        

type PidType = SFixed 2 14 -- 16 bit signed fixed float number -2 <= x < 2

topEntity :: (PidType , PidType , PidType , PidType , PidType ) 
             -> Signal PidType -> Signal PidType 
topEntity = pidProc

-----------------
--test

pidSim pidArg setvalue process = processOut where 
     processOut = process $ topEntity pidArg error
     error = uncurry (-) <$> bundle (setvalue ,  processOut )

modelSim = topEntity (0,0,0.25,0.01,0) -- we use pid as process to regulate

testout = sampleN 20 $ pidSim (0,0,0.25,0.01,0) (signal 0.5) modelSim