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>

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);
};

void FARSA_WSIM_API wheeledRobotsComputeKinematicMovement(wMatrix &mtr, real leftWheelVelocity, real rightWheelVelocity, real wheelr, real axletrack, real timestep);

bool FARSA_WSIM_API wheeledRobotsComputeWheelsSpeed(const wMatrix& start, const wMatrix& end, real wheelr, real axletrack, real timestep, real& leftWheelVelocity, real& rightWheelVelocity);

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 ) const;
    void getSpeeds( double& sp1, double& sp2 ) const;
    void getDesiredSpeeds( QVector<double>& speeds ) const;
    void getDesiredSpeeds( double& sp1, double& sp2 ) const;
    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 ) const;
    void getSpeedLimits( double& minSpeed1, double& minSpeed2, double& maxSpeed1, double& maxSpeed2 ) const;
    void setMaxTorque( double maxTorque );
    double getMaxTorque() const;
private:
    QVector<PhyDOF*> motorsv;
    QVector<double> desiredVel;
    QVector<double> minVel;
    QVector<double> maxVel;
};

class PhyMarXbot;
class PhyFixed;

class FARSA_WSIM_API MarXbotAttachmentDeviceMotorController : public MotorController
{
public:
    enum Status {
        Open,
        HalfClosed,
        Closed
    };

public:
    MarXbotAttachmentDeviceMotorController(PhyMarXbot* robot);

    virtual ~MarXbotAttachmentDeviceMotorController();

    virtual void update();

    bool attachmentDeviceEnabled() const;

    void setMaxVelocity(double speed);

    double getMaxVelocity() const;

    void setDesiredPosition(double pos);

    double getDesiredPosition() const;

    double getPosition() const;

    void setDesiredVelocity(double vel);

    double getDesiredVelocity() const;

    double getVelocity() const;

    void setDesiredStatus(Status status);

    Status getDesiredStatus() const
    {
        return m_desiredStatus;
    }

    Status getStatus() const
    {
        return m_status;
    }

    bool attachmentPossible() const
    {
        return (tryToAttach() != NULL);
    }

    bool attachedToRobot() const
    {
        return (m_attachedRobot != NULL);
    }

    PhyMarXbot* attachedRobot()
    {
        return m_attachedRobot;
    }

    const PhyMarXbot* attachedRobot() const
    {
        return m_attachedRobot;
    }

    bool otherAttachedToUs() const
    {
        return !m_otherAttachedRobots.isEmpty();
    }

    QVector<PhyMarXbot*> otherAttachedRobots()
    {
        return m_otherAttachedRobots;
    }

private:
    PhyMarXbot* tryToAttach() const;

    void attachmentDeviceAboutToBeDestroyed();

    PhyMarXbot* const m_robot;

    Status m_status;

    Status m_desiredStatus;

    PhyJoint* m_joint;

    PhyMarXbot* m_attachedRobot;

    QVector<PhyMarXbot*> m_otherAttachedRobots;

    friend class PhyMarXbot;
};

} // end namespace farsa

#endif