HeadingPIDController.h

#ifndef __HeadingPIDController_H
#define __HeadingPIDController_H
 
#include <fhsim/simobject/SimObject.h>
#include <string>
#include <Eigen/Eigen>
using namespace Eigen;
 
 
namespace Ship{
    class HeadingPIDController: public SimObject
    {
    public:
        HeadingPIDController(const std::string& simObjectName, ISimObjectCreator* const creator); 
        virtual void OdeFcn(const double T, const double* const X, double* const XDot) const; 
 
        const double* Torque(const double T, const double *const X);
        const double* AngleDiff(const double T, const double *const X);
 
    #ifdef FH_VISUALIZATION
        virtual void RenderInit(Ogre::Root* const ogreRoot, ISimObjectCreator* const creator){} 
 
        virtual void RenderUpdate(const double T, const double* const X){} 
    #endif
 
    protected:
        double Kp,Kd,Ki;
        double T,angleDiff;
        ISignalPort* m_InCurrentHeading;    
        ISignalPort* m_InCurrentYawRate;    
        ISignalPort* m_InDesiredHeading;    
        ISignalPort* m_InDesiredYawRate;    
        int m_IErrorIntegral;   
 
        double HeadingDifference( double from, double to) const;
    };
}
#endif