SimulinkのPID Controllerブロックを用いると、やりたいことができるのではないでしょうか。
以下に、無料の学習コンテンツもあります。
このコンテンツを利用するにはSimulinkに加えて、Control System Toolbox(こちらはすでにお持ちでしょうか)と、Simulink Control Designというツールボックスも必要になります。PID Controllerブロックは、Simulinkがあれば使うことができます。
コード生成をしたい場合は、Simulink Coderや、必要に応じてEmbedded Coderというツールボックスが必要になります。