worldsim/include/motorcontrollers.h

/********************************************************************************
 *  WorldSim -- library for robot simulations                                   *
 *  Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.it>                *
 *                                                                              *
 *  This program is free software; you can redistribute it and/or modify        *
 *  it under the terms of the GNU General Public License as published by        *
 *  the Free Software Foundation; either version 2 of the License, or           *
 *  (at your option) any later version.                                         *
 *                                                                              *
 *  This program is distributed in the hope that it will be useful,             *
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of              *
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               *
 *  GNU General Public License for more details.                                *
 *                                                                              *
 *  You should have received a copy of the GNU General Public License           *
 *  along with this program; if not, write to the Free Software                 *
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA  *
 ********************************************************************************/

#ifndef MOTORCONTROLLERS_H
#define MOTORCONTROLLERS_H

#include "worldsimconfig.h"
#include "phyjoint.h"
#include "ownable.h"
#include <QVector>

// All this stuff is to get rid of annoying warnings...
#ifdef __GNUC__
#if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 5)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#else
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif
#endif
#ifdef FARSA_USE_YARP_AND_ICUB
    #include <yarp/dev/ControlBoardInterfaces.h>
    #include <yarp/dev/ControlBoardPid.h>
#endif
#ifdef __GNUC__
#if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 5)
#pragma GCC diagnostic pop
#else
#pragma GCC diagnostic warning "-Wunused-parameter"
#endif
#endif

namespace farsa {

class World;

class FARSA_WSIM_API wPID {
public:
    wPID();
    double pidloop( double currentvalue );
    void reset();
    double p_gain;
    double i_gain;
    double d_gain;
    double acc_ff;
    double fri_ff;
    double vel_ff;
    double bias;
    double accel_r;
    double setpt;
    double min;
    double max;
    double slew;
private:
    double this_target;
    double next_target;
    double integral;
    double last_error;
    double last_output;
};

class FARSA_WSIM_API MotorController : public Ownable {
public:
    MotorController( World* world );
    virtual ~MotorController();
    virtual void update() = 0;
    bool isEnabled() {
        return enabled;
    };
    void setEnabled( bool b ) {
        enabled = b;
    };
    World* world() {
        return worldv;
    };

private:
    World* worldv;
    bool enabled;

    MotorController(const MotorController& other);

    MotorController& operator=(const MotorController& other);
};

class FARSA_WSIM_API MultiMotorController : public MotorController
#ifdef FARSA_USE_YARP_AND_ICUB
, public yarp::dev::DeviceDriver, public yarp::dev::IPositionControl, public yarp::dev::IVelocityControl, public yarp::dev::IControlMode, public yarp::dev::IEncoders, public yarp::dev::IControlLimits, public yarp::dev::IControlLimitsRaw
#endif
{
public:
    MultiMotorController( QVector<PhyDOF*> motors, World* world );
    virtual ~MultiMotorController();
    QVector<PhyDOF*>& motors() {
        return motorsv;
    };
    QVector<wPID*>& positionPIDs() {
        return pos_pids;
    };
    bool setEnableLimitsRaw( int axis, bool b );
    virtual void update();
    QVector<wPID*>& velocityPIDs() {
        return vel_pids;
    };

#ifdef FARSA_USE_YARP_AND_ICUB

    virtual bool close( );
    virtual bool configure( yarp::os::Searchable &config );
    virtual bool open( yarp::os::Searchable &config );
#endif


    virtual bool getAxes(int *ax);
    virtual bool getRefAcceleration(int j, double *acc);
    virtual bool getRefAccelerations(double *accs);
    virtual bool setRefAcceleration(int j, double acc);
    virtual bool setRefAccelerations(const double *accs);
    virtual bool stop(int j);
    virtual bool stop();

    virtual bool checkMotionDone(int j, bool *flag);
    virtual bool checkMotionDone(bool *flag);
    virtual bool getRefSpeed(int j, double *ref);
    virtual bool getRefSpeeds(double *spds);
    virtual bool positionMove(int j, double ref);
    virtual bool positionMove(const double *refs);
    virtual bool relativeMove(int j, double delta);
    virtual bool relativeMove(const double *deltas);
    virtual bool setPositionMode();
    virtual bool setRefSpeed(int j, double sp);
    virtual bool setRefSpeeds(const double *spds);

    virtual bool setVelocityMode();
    virtual bool velocityMove(int j, double sp);
    virtual bool velocityMove(const double *sp);

    virtual bool getControlMode( int j, int* mode );
    virtual bool getControlModes( int* modes );
    virtual bool setImpedancePositionMode(int) { return false; }
    virtual bool setImpedanceVelocityMode(int) { return false; }
    virtual bool setOpenLoopMode(int j);
    virtual bool setPositionMode( int j );
    virtual bool setTorqueMode( int j );
    virtual bool setVelocityMode(int j );

    virtual bool getEncoder( int j, double *v );
    virtual bool getEncoders(double *encs);
    virtual bool getEncoderAcceleration( int j, double *spds );
    virtual bool getEncoderAccelerations( double *accs );
    virtual bool getEncoderSpeed( int j, double *sp );
    virtual bool getEncoderSpeeds( double *spds );
    virtual bool resetEncoder( int j );
    virtual bool resetEncoders();
    virtual bool setEncoder( int j, double val );
    virtual bool setEncoders( const double *vals );

    virtual bool getLimits( int axis, double *min, double *max );
    virtual bool setLimits( int axis, double min, double max );

    virtual bool getLimitsRaw( int axis, double *min, double *max );
    virtual bool setLimitsRaw( int axis, double min, double max );
private:
    QVector<PhyDOF*> motorsv;
    //--- Common Variable among Control Interfaces
    QVector<int> controlModes;
    QVector<real> refsacc;

    //--- IPositionControl Variables
    QVector<real> refs;
    QVector<real> refsvel;
    QVector<real> xfx0, x0, dx0, t, T;
    QVector<bool> stops;
    QVector<wPID*> pos_pids;
    QVector<real> lastErrors, lastIs, lastTorques;
    real fromRawPosition( int i );
    real toRawPosition( real noRawPos, int i );
    //--- END IPositionControl Variables

    //--- IVelocityControl Variables
    QVector<real> desiredVel;
    QVector<wPID*> vel_pids;
    //--- END IVelocityControl Variables

    //--- IControlLimits Variables
    QVector<double> loLimits;
    QVector<double> hiLimits;
    //--- END IControlLimits Variables
};

class FARSA_WSIM_API WheelMotorController : public MotorController {
public:
    WheelMotorController( QVector<PhyDOF*> wheels, World* world );
    virtual ~WheelMotorController();
    QVector<PhyDOF*>& wheels() {
        return motorsv;
    };
    virtual void update();
    void setSpeeds( QVector<double> speeds );
    void setSpeeds( double sp1, double sp2 );
    void getSpeeds( QVector<double>& speeds );
    void getSpeeds( double& sp1, double& sp2 );
    void setSpeedLimits( QVector<double> minSpeeds, QVector<double> maxSpeeds );
    void setSpeedLimits( double minSpeed1, double minSpeed2, double maxSpeed1, double maxSpeed2 );
    void getSpeedLimits( QVector<double>& minSpeeds, QVector<double>& maxSpeeds );
    void getSpeedLimits( double& minSpeed1, double& minSpeed2, double& maxSpeed1, double& maxSpeed2 );
private:
    QVector<PhyDOF*> motorsv;
    QVector<double> desiredVel;
    QVector<double> minVel;
    QVector<double> maxVel;
};

} // end namespace farsa

#endif