mhipa10.h

/*************************************************************************
        
        date            : August 2008
        copyright       : (C) 2005 J.D. Yamokoski
        email   : yamokosk+gnu at gmail dot com

 This library is free software; you can redistribute it and/or
 modify it under the terms of the GNU Lesser General Public License as
 published by the Free Software Foundation; either version 2.1 of the License,
 or (at your option) any later version. The text of the GNU Lesser General
 Public License is included with this library in the file LICENSE.TXT.
 
 This library 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 file LICENSE.TXT for
 more details.
 
 *************************************************************************/

#ifndef MITSUBISHIPA10_
#define MITSUBISHIPA10_

#include <exception>
#include <vector>

#include <hydroutil/context.h>

namespace hydrointerfaces
{

class MitsubishiPA10
{
public:
        static const float maxRatedMotorTorque[7];
        
        struct LinkConfig
        {
                std::string Name;
                float JointAngle;
                float JointOffset;
                float LinkLength;
                float TwistAngle;
                float MinJointAngle;
                float MaxJointAngle;
                float MaxJointVelocity;
                float JointAngleOffset;
                float LinkMass;
                float CenterOfMass[3];
                float Inertia[6]; // Ixx, Ixy, Ixz, Iyy, Iyz, Izz
                float MaxTorque;
                float MotorInertia;
                float GearRatio;
                float ViscousFriction;
                float CoulombFriction;
                bool Immobile;  
        };
        
        struct ManipulatorConfig
        {
                bool AngleInDegree;
                bool IgnoreMotorDynamics;
                std::vector< LinkConfig > links;
        };
        
        struct ServoData
        {
                ServoData() :
                        isPowered(false),
                        isControllable(false),
                        inWarning(true),
                        inFault(true),
                        angle(0.0),
                        speed(0.0),
                        torque(0.0)
                { }
                bool isPowered;
                bool isControllable;
                bool inWarning;
                bool inFault;
                std::vector< std::string > msg;
                double angle;
                float speed;
                float torque;
                std::string toString() const;
        };

        struct ManipulatorData
        {
                ManipulatorData(unsigned int servonum=7) :
                        seconds(0),
                        useconds(0),
                        isControllable(false),
                        powerOutputOn(false),
                        nominalPowerSourceTemp(false),
                        limitSwitchOpen(false),
                        estopSwitchOpen(false),
                        deadmanSwitchClosed(false),
                        teachingSwitchOpen(false),
                        inWarning(true),
                        inFault(true)
                {
                        servoData.resize(servonum);
                }
                int seconds;
                int useconds;
                bool isControllable;
                bool powerOutputOn;
                bool nominalPowerSourceTemp;
                bool limitSwitchOpen;
                bool estopSwitchOpen;
                bool deadmanSwitchClosed;
                bool teachingSwitchOpen;
                bool inWarning;
                bool inFault;
                std::vector< std::string > msg;
                float mobility;
                std::vector<ServoData> servoData;
                std::string toString() const;
        };

        struct ServoCmd
        {
                ServoCmd(bool servoON=true, bool brakeON=true, bool isTrqCmd=false, float cmdValue=0.0) :
                        servoON(servoON), brakeON(brakeON), isTorqueCmd(isTrqCmd), cmdValue(cmdValue) {}
                bool servoON;
                bool brakeON;
                bool isTorqueCmd;
                float cmdValue;
                std::string toString() const;
        };

        struct ManipulatorCommand
        {
                int seconds;
                int useconds;
                bool enableMotion;
                std::vector<ServoCmd> servoCmds;
                std::string toString() const;
        };

        class Exception : public std::exception
        {
        public:
            Exception(const char *message) : message_(message) {}
            Exception(const std::string &message) : message_(message) {}
            virtual ~Exception() throw() {}
            virtual const char* what() const throw() { return message_.c_str(); }
        protected:
            std::string  message_;
        };
        
        MitsubishiPA10() {};
        virtual ~MitsubishiPA10() {}

        virtual void enable()=0;

        virtual bool read( ManipulatorData &data )=0;

        virtual void write( const ManipulatorCommand& command )=0;

        virtual void getStatus( std::string &status, bool &isWarn, bool &isFault )=0;

        virtual void startMotionControl() = 0;
        virtual void stopMotionControl() = 0;
        
        // Returns whether the PA10 is accepting motion control commands
        virtual bool isMotionEnabled() = 0;

        virtual void checkCommand( ManipulatorCommand& command ) = 0;
        
        /*virtual void applyHardwareLimits( double &maxForwardSpeed,
                                      double &maxReverseSpeed,
                                      double &maxTurnrate,
                                      double &maxLateralAcceleration )=0;*/
        static ManipulatorCommand createBrakeONCmd();
        static ManipulatorCommand createBrakeOFFNoMotionCmd();
};

class MitsubishiPA10Factory
{
public:
        virtual ~MitsubishiPA10Factory() {};
        virtual MitsubishiPA10* createDriver( const MitsubishiPA10::ManipulatorConfig& config, const hydroutil::Context& context ) const = 0;
};

} // namespace

// Function for dynamically instantiating drivers.
// A driver must have a function like so:
// extern "C" {
//     hydrointerfaces::SegwayRmpFactory *createMitsubishiPA10DriverFactory();
// }
typedef hydrointerfaces::MitsubishiPA10Factory *MitsubishiPA10DriverFactoryMakerFunc();


#endif /*MITSUBISHIPA10_*/