MitsubishiH7.h

Go to the documentation of this file.

//
// Copyright (c) 2009, Markus Rickert
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
 
#ifndef _RL_HAL_MITSUBISHIH7_H_
#define _RL_HAL_MITSUBISHIH7_H_
 
#include <string>
#include <rl/math/Transform.h>
 
#include "CartesianPositionActuator.h"
#include "CartesianPositionSensor.h"
#include "Gripper.h"
#include "JointPositionActuator.h"
#include "JointPositionSensor.h"
#include "types.h"
 
namespace rl
{
    namespace hal
    {
        class TcpSocket;
        class UdpSocket;
        
        class MitsubishiH7 : public CartesianPositionActuator, public CartesianPositionSensor, public Gripper, public JointPositionActuator, public JointPositionSensor
        {
        public:
            enum Mode
            {
                MODE_JOINT = 1,
                MODE_POSE = 0,
                MODE_PULSE = 2
            };
            
            enum HandSts
            {
                HANDSTS_NOTUSED = -1,
                HANDSTS_OPEN = 1,
                HANDSTS_CLOSED = 2
            };
            
            enum HandType
            {
                HANDTYPE_NOTUSED = -1,
                HANDTYPE_SINGLE = 0,
                HANDTYPE_DOUBLE = 1
            };
            
            enum TaskCond
            {
                TASKCOND_START,
                TASKCOND_ALWAYS,
                TASKCOND_ERROR
            };
            
            struct EditSts
            {
                bool isEditing;
                bool isRunning;
                bool isChanged;
            };
            
            struct Hand
            {
                int outputNo;
                HandSts handSts;
                HandType handType;
            };
            
            typedef bool MechInfo[3];
            
            struct RunSts
            {
                bool isRepeat;
                bool isCycleStopOff;
                bool isMlockOn;
                bool isTeach;
                bool isTeachRunning;
                bool isServoOn;
                bool isRun;
                bool isOperationEnable;
            };
            
            struct StopSts
            {
                bool isEmgStop;
                bool isStop;
                bool isWait;
                bool isStopSignalOff;
                bool isProgramSelectEnable;
                bool isReserve;
                bool isPseudoInput;
            };
            
            struct CalibState
            {
                bool isDefined;
                bool isAxis[8];
            };
            
            typedef Hand HandState[8];
            
            struct RunState
            {
                char programName[256];
                int lineNo;
                int override;
                EditSts editSts;
                RunSts runSts;
                StopSts stopSts;
                int errorNo;
                int stepNo;
                MechInfo mechInfo;
                char taskPrgName[256];
                bool isTaskModeCycle;
                TaskCond taskCond;
                int taskPri;
                int mechNo;
            };
            
            struct StopSignalState
            {
                bool isTb;
                bool isPc;
                bool isIo;
                bool isOp;
                bool isIoEmg;
                bool isOpEmg;
                bool isTbEmg;
            };
            
            struct StopState
            {
                RunSts runSts;
                StopSts stopSts;
                int errorNo;
                int stepNo;
                int mechNo;
            };
            
            MitsubishiH7(
                const ::std::size_t& dof,
                const ::std::string& server,
                const ::std::string& client,
                const unsigned short int& tcp = 10001,
                const unsigned short int& udp = 10000,
                const Mode& mode = MODE_JOINT,
                const uint16_t& haltIoData = 0x00AA,
                const uint16_t& releaseIoData = 0x00A6,
                const uint16_t& shutIoData = 0x000A9
            );
            
            virtual ~MitsubishiH7();
            
            void calibCmd(CalibState& state) const;
            
            void close();
            
            void cntlCmd(const bool& doOn) const;
            
            void datinstCmd(const ::std::string& j1, const ::std::string& j2, const ::std::string& j3, const ::std::string& j4, const ::std::string& j5, const ::std::string& j6, const ::std::string& checksum) const;
            
            void datinstCmd(const ::std::string& j1, const ::std::string& j2, const ::std::string& j3, const ::std::string& j4, const ::std::string& j5, const ::std::string& j6, const ::std::string& j7, const ::std::string& j8, const ::std::string& checksum) const;
            
            void dstateCmd(StopState& state) const;
            
            void eclrCmd() const;
            
            void emdatCmd(const ::std::string& program) const;
            
            ::std::string errormesCmd(const int& errorNo) const;
            
            void execCmd(const ::std::string& instruction) const;
            
            void getCartesianPosition(::rl::math::Transform& x) const;
            
            void getCurrentFeedback(::Eigen::Matrix< int32_t, ::Eigen::Dynamic, 1 >& c) const;
            
            ::std::size_t getFilter() const;
            
            uint16_t getIoData() const;
            
            void getJointPosition(::rl::math::Vector& q) const;
            
            Mode getMode() const;
            
            void getMotorPulse(::Eigen::Matrix< int32_t, ::Eigen::Dynamic, 1 >& p) const;
            
            void halt();
            
            void hndCmd(const bool& doOpen, const int& handNo) const;
            
            void hndstsCmd(HandState& state) const;
            
            void inEqualsCmd(const ::std::size_t& inNo, const ::std::string& inVal) const;
            
            void loadCmd(const ::std::string& programName) const;
            
            void loadProgram(const ::std::string& name, const ::std::string& program) const;
            
            void newCmd() const;
            
            void open();
            
            void outEqualsCmd(const ::std::size_t& outNo, const ::std::string& outVal) const;
            
            void release();
            
            void rstalrmCmd() const;
            
            void rstpwrCmd() const;
            
            void runCmd(const ::std::string& programName, const bool& doModeCycle = false) const;
            
            void saveCmd() const;
            
            void setCartesianPosition(const ::rl::math::Transform& x);
            
            void setFilter(const ::std::size_t& filter);
            
            void setInput(const uint16_t& bitTop);
            
            void setJointPosition(const ::rl::math::Vector& q);
            
            void setMode(const Mode& mode);
            
            void setMotorPulse(const ::Eigen::Matrix< int32_t, ::Eigen::Dynamic, 1 >& p);
            
            void setOutput(const uint16_t& bitTop, const uint16_t& bitMask, const uint16_t& ioData);
            
            void shut();
            
            void slotinitCmd() const;
            
            void srvCmd(const bool& doOn) const;
            
            void start();
            
            void startProgram(const ::std::string& name) const;
            
            void stateCmd(RunState& state) const;
            
            void step();
            
            void stopCmd() const;
            
            void stop();
            
            void stopProgram() const;
            
            void stpsigCmd(StopSignalState& state) const;
            
        protected:
            struct MxtWorld
            {
                float x;
                float y;
                float z;
                float a;
                float b;
                float c;
                float l1;
                float l2;
            };
            
            struct MxtJoint
            {   
                float j1;
                float j2;
                float j3;
                float j4;
                float j5;
                float j6;
                float j7;
                float j8;
            };
            
            struct MxtPose
            {
                MxtWorld w;
                uint32_t sflg1;
                uint32_t sflg2;
            };
            
            struct MxtPulse
            {
                int32_t p1;
                int32_t p2;
                int32_t p3;
                int32_t p4;
                int32_t p5;
                int32_t p6;
                int32_t p7;
                int32_t p8;
            };
            
            struct MxtCommand
            {
                union MxtData
                {
                    MxtPose pos;
                    MxtJoint jnt;
                    MxtPulse pls;
                    int32_t lng[8];
                };
                uint16_t command;
                uint16_t sendType;
                uint16_t recvType;
                uint16_t reserve;
                MxtData dat;
                uint16_t sendIoType;
                uint16_t recvIoType;
                uint16_t bitTop;
                uint16_t bitMask;
                uint16_t ioData;
                uint16_t tCount;
                uint32_t cCount;
                uint16_t recvType1;
                uint16_t reserve1;
                MxtData dat1;
                uint16_t recvType2;
                uint16_t reserve2;
                MxtData dat2;
                uint16_t recvType3;
                uint16_t reserve3;
                MxtData dat3;
            };
            
        private:
            ::std::string client;
            
            ::std::size_t filter;
            
            uint16_t haltIoData;
            
            MxtCommand in;
            
            Mode mode;
            
            MxtCommand out;
            
            uint16_t releaseIoData;
            
            ::std::string server;
            
            uint16_t shutIoData;
            
            TcpSocket* tcp;
            
            UdpSocket* udp;
        };
    }
}
 
#endif // _RL_HAL_MITSUBISHIH7_H_