mtsPID.h

Go to the documentation of this file.

/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*-    */
/* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */

/*
  Author(s):  Zihan Chen
  Created on: 2013-02-22

  (C) Copyright 2013-2015 Johns Hopkins University (JHU), All Rights Reserved.

--- begin cisst license - do not edit ---

This software is provided "as is" under an open source license, with
no warranty.  The complete license can be found in license.txt and
http://www.cisst.org/cisst/license.txt.

--- end cisst license ---
*/


#ifndef _mtsPID_h
#define _mtsPID_h

#include <cisstMultiTask/mtsTaskPeriodic.h>
#include <cisstParameterTypes/prmForceTorqueJointSet.h>
#include <cisstParameterTypes/prmPositionJointGet.h>
#include <cisstParameterTypes/prmPositionJointSet.h>
#include <cisstParameterTypes/prmVelocityJointGet.h>
#include <cisstParameterTypes/prmStateJoint.h>
#include <cisstParameterTypes/prmJointType.h>

#include <sawControllers/sawControllersRevision.h>

#include <sawControllers/sawControllersExport.h>

class CISST_EXPORT mtsPID: public mtsTaskPeriodic
{
    CMN_DECLARE_SERVICES(CMN_DYNAMIC_CREATION_ONEARG, CMN_LOG_ALLOW_DEFAULT);

protected:
    // Required interface
    struct InterfaceRobotTorque {
        mtsFunctionRead GetJointType;
        mtsFunctionRead GetFeedbackPosition;
        mtsFunctionRead GetFeedbackVelocity;
        mtsFunctionRead GetFeedbackTorque;
        mtsFunctionWrite SetTorque;
    } Robot;

    int Counter;

    vctDoubleVec Kp;
    vctDoubleVec Kd;
    vctDoubleVec Ki;
    vctDoubleVec Offset;
    vctDoubleVec JointLowerLimit;
    vctDoubleVec JointUpperLimit;
    bool CheckJointLimit;
    vctBoolVec mPreviousJointLimitFlag, mJointLimitFlag;


    // TODO: change to prmPositionJointGet
    vctDoubleVec FeedbackPosition;
    vctDoubleVec DesiredPosition;
    vctDoubleVec FeedbackTorque;
    vctDoubleVec DesiredTorque;
    vctDoubleVec FeedbackVelocity;
    vctDoubleVec DesiredVelocity;
    vctDoubleVec Torque;

    prmJointTypeVec JointType;
    prmPositionJointGet FeedbackPositionParam;
    prmPositionJointGet FeedbackPositionPreviousParam;
    prmPositionJointSet DesiredPositionParam;
    prmForceTorqueJointSet prmDesiredTrq;
    prmVelocityJointGet FeedbackVelocityParam;
    prmForceTorqueJointSet TorqueParam;
    prmStateJoint mStateJoint, mStateJointDesired;

    vctDoubleVec Error;
    vctDoubleVec ErrorAbsolute;

    vctDoubleVec dError;
    vctDoubleVec iError;

    vctDoubleVec oldError;
    vctDoubleVec oldDesiredPos;

    vctDoubleVec minIErrorLimit;
    vctDoubleVec maxIErrorLimit;

    vctDoubleVec forgetIError;

    vctDoubleVec nonlinear;

    vctDoubleVec DeadBand;

    bool Enabled;

    vctBoolVec TorqueMode;

    bool mEnableTrackingError;
    vctDoubleVec mTrackingErrorTolerances;
    vctBoolVec mPreviousTrackingErrorFlag, mTrackingErrorFlag;

    mtsStateTable ConfigurationStateTable;

    struct {
        mtsFunctionWrite Enabled;
        // !Joint limit event
        mtsFunctionWrite JointLimit;
    } Events;

    struct {
        mtsFunctionWrite Status;
        mtsFunctionWrite Warning;
        mtsFunctionWrite Error;
    } MessageEvents;

    void ResetController(void);

    void SetDesiredPositions(const prmPositionJointSet & prmPos);
    void SetDesiredTorques(const prmForceTorqueJointSet& prmTrq);

    void SetupInterfaces(void);

    void Enable(const bool & enable);

    void EnableTorqueMode(const vctBoolVec & enable);

    void SetTrackingErrorTolerances(const vctDoubleVec & tolerances);

    void ErrorEventHandler(const std::string & message);

public:

    mtsPID(const std::string & taskname,
           const double period);
    mtsPID(const mtsTaskPeriodicConstructorArg & arg);
    ~mtsPID(){}

    void Configure(const std::string& filename);
    void Startup(void);
    void Run(void);
    void Cleanup(void);


protected:
    void SetPGain(const vctDoubleVec & pgain);

    void SetDGain(const vctDoubleVec & dgain);

    void SetIGain(const vctDoubleVec & igain);

    void SetJointLowerLimit(const vctDoubleVec & lowerLimit);

    void SetJointUpperLimit(const vctDoubleVec & upperLimit);


    void SetMinIErrorLimit(const vctDoubleVec & iminlim);


    void SetMaxIErrorLimit(const vctDoubleVec & imaxlim);

    void SetForgetIError(const double & forget);

};

CMN_DECLARE_SERVICES_INSTANTIATION(mtsPID);

#endif  // _mtsPID_h