3mxl.h
Go to the documentation of this file.
00001 // Dynamixel control code - header file
00002 // Copyright (c) 2010 Eelko van Breda
00003 // based on dynamixel.h (Erik Schuitema & Eelko van Breda)
00004 // Delft University of Technology
00005 // www.dbl.tudelft.nl
00006 
00007 #ifndef __3MXL_H_INCLUDED__
00008 #define __3MXL_H_INCLUDED__
00009 
00010 #include <stdio.h>
00011 #include <string.h>
00012 #include <iostream>
00013 #include <sstream>
00014 #include <errno.h>
00015 #include <math.h>
00016 #include <threemxl/platform/io/logging/Log2.h>
00017 
00018 
00019 #include "../Byte.h"
00020 #include "../CDxlGeneric.h"
00021 #include "../CDxlPacket.hpp"
00022 #include "3mxlControlTable.h"
00023 
00024 #define __DBG__
00025 
00026 // Dynamixel error codes: they are all > 0
00027 // When returned < 0 from any function, this means the
00028 // error code was returned internally by CDynamixel and the
00029 // code does not originate from an actual motor.
00030 #define DXL_ERR_INPUT_VOLTAGE                   1
00031 #define DXL_ERR_ANGLE_LIMIT                             2
00032 #define DXL_ERR_OVERHEATING                             4
00033 #define DXL_ERR_RANGE                                   8
00034 #define DXL_ERR_CHECKSUM                                16
00035 #define DXL_ERR_OVERLOAD                                32
00036 #define DXL_ERR_INSTRUCTION                             64
00037 
00038 #define INITIAL_RETURN_DELAY_TIME               500
00039 #define INITIAL_TORQUE_LIMIT                    1.0
00040 #define INITIAL_3MXL_MODE                               POSITION_MODE
00041 #define INITIAL_WATCHDOG_MODE                   0
00042 #define INITIAL_WATCHDOG_TIME                   100
00043 #define INITIAL_WATCHDOG_MULT                   1
00044 #define INITIAL_STATUS_RETURN_LEVEL             1
00045 #define INITIAL_MOTOR_CONSTANT                  0
00046 #define INITIAL_GEARBOX_RATIO                   1
00047 #define INITIAL_ENCODER_COUNT_MOTOR             0
00048 #define INITIAL_ENCODER_COUNT_JOINT             0
00049 #define INITIAL_OFFSET_MOTOR                    0
00050 #define INITIAL_OFFSET_JOINT                    0
00051 #define INITIAL_MAX_UNINITIALIZED_MOTOR_CURRENT 0
00052 #define INITIAL_MAX_MOTOR_CURRENT               0
00053 #define INITIAL_MIN_JOINT_ANGLE                 0
00054 #define INITIAL_MAX_JOINT_ANGLE                 0
00055 #define INITIAL_SPRING_STIFFNESS                0
00056 #define INITIAL_ZERO_SPRING_LENGTH              0
00057 #define INITIAL_JOINT_DIRECTION                 1
00058 
00059 
00060 #define PI_DOUBLE 3.1415926535897932
00061 
00062 
00063 // Dynamixel on, off and toggle convention
00064 #define DXL_OFF                                 0
00065 #define DXL_ON                                  1
00066 #define DXL_TOGGLE                              2
00067 
00068 //enum E3mxlMode
00069 //{
00070 //      mxlMode_Uninitialised,  //startup mode
00071 //      mxrMode_Torque,                 //torque control
00072 //      mxlMode_Speed,                  //speed control
00073 //      mxlMode_SpeedPosition,  //get to a position with the given speed as max speed
00074 //      mxlMode_PositionSpeed,  //get to a position and have the speed at that position
00075 //      mxlMode_SEA_PSTPD               //setPosSpeedTorquePPosDPos mode
00076 //};
00077 
00078 
00084 class C3mxl: public CDxlGeneric
00085 {
00086         protected:
00087 
00088                 // Internal motor configuration
00089                 CDxlConfig      mConfig;
00090                 CLog2           mLog;
00091                 // In SI units
00092                 double          mVoltage;               // Motor voltage [V]
00093                 double          mBusVoltage;            // Bus voltage [V]
00094                 double          mCurrentADCVoltage;     // Current ADC voltage [V]
00095                 double          mAnalog1Voltage;        // Analog input 1 voltage [V]
00096                 double          mAnalog2Voltage;        // Analog input 2 voltage [V]
00097                 double          mAnalog3Voltage;        // Analog input 3 voltage [V]
00098                 double          mAnalog4Voltage;        // Analog input 4 voltage [V]
00099                 double          mCurrent;               // Current [A]
00100                 double          mTorque;                // Torque [Nm]
00101                 double          mPosition;              // Position [rad]
00102                 double          mLinearPosition;        // Position [m]
00103                 double          mSpeed;                 // Speed [rad/s]
00104                 double          mAcceleration;  // Acceleration [rad/s^2]
00105                 double          mLinearAcceleration;    // Acceleration [m/s^2]
00106                 unsigned char           mStatus;                        // Status value of 3mxl
00107                 unsigned char           mMotorInitialized;      // this value tells you that the motor did the initialization procedure
00108 
00109                 TMxlLog mMxlLog;
00110                 BYTE mMxlMode;
00111 
00112 //              double          mLoad;                  // Current load expressed as a ratio of the maximum torque [dimensionless]
00113 //              double          mTemperature;   // Temperature [deg. C]
00114 
00115                 // Return level. Can be one of the following:
00116                 // 0: respond to Ping only
00117                 // 1: respond only when asked for data
00118                 // 2: respond always
00119 
00120                 // Clockwise and counter-clockwise angle limits.
00121                 // These are NOT saved in SI units but as Dynamixel WORD values, because
00122                 // we want to use the exact CWAngleLimit and CCWAngleLimit in setSpeed()
00123                 // as they are present in the control table of the Dynamixel (also as WORD values).
00124                 // Otherwise, we may set a slightly larger or smaller goal position in setSpeed() and
00125                 // encounter an angle limit error. Then setSpeed() would fail completely.
00126 
00127 //              unsigned char   m3mxlMode;
00128 //              BYTE            mWatchdogMode;
00129 //              BYTE            mWatchdogTime;
00130 //              BYTE            mWatchdogMult;
00131 //              BYTE            mStatusReturnLevel;
00132 //              double          mMotorConstant;
00133 //              double          mGearboxRatio;
00134 //              COptionWord     mEncoderCountMotor;                             //total nr of encoder counts motor
00135 //              COptionDouble   mOffsetMotor;                           //zero position of the motor in rad only used temporarily
00136 //              double          mMinMotorAngleLimit;
00137 //              double          mMaxMotorAngleLimit;
00138 //              double          mMaxUninitialisedMotorCurrent;
00139 //              double          mMaxMotorCurrent;
00140 //              int                     mMotorDirection;                                // Should be either 1.0 or -1.0 and is used as internal multiplication factor!
00141 //              COptionWord     mEncoderCountJoint;                             //total number of encoder counts joint
00142 //              COptionDouble   mOffsetJoint;                           //zero position of the joint in rad only used temporarily
00143 //              double          mMinJointAngle;
00144 //              double          mMaxJointAngle;
00145 //              double          mZeroLengthSpring;
00146 //              double          mSpringStiffness;
00147 //              int                     mJointDirection;
00148 
00149                 int                     clip(const int value, const int min, const int max)
00150                 {
00151                         int result = value;
00152                         if (result < min)
00153                                 result = min;
00154                         if (result > max)
00155                                 result = max;
00156                         return result;
00157                 }
00158 
00159                 int                     round(double val)
00160                 {
00161                         return (int)floor(val+0.5);
00162                 }
00163 
00164                 // Internal conversions
00165                 double          mxlCurrentToInternalCurrent(WORD current);
00166                 WORD            internalCurrentToMxlCurrent(double current);
00167                 double          mxlVoltageToInternalVoltage(WORD voltage);
00168                 WORD            internalVoltageToMxlVoltage(double voltage);
00169                 double          mxlPosToInternalPos(WORD pos);
00170                 WORD            internalPosToMxlPos(double pos);
00171                 double          mxlSpeedToInternalSpeed(WORD speed);
00172                 WORD            internalSpeedToMxlSpeed(double speed);
00173                 double          mxlAccelerationToInternalAcceleration(WORD acceleration);
00174                 WORD            internalAccelerationToMxlAcceleration(double acceleration);
00175                 double          mxlLinearPosToInternalLinearPos(DWORD pos);
00176                 DWORD           internalLinearPosToMxlLinearPos(double pos);
00177                 double          mxlLinearSpeedToInternalLinearSpeed(WORD speed);
00178                 WORD            internalLinearSpeedToMxlLinearSpeed(double speed);
00179                 double          mxlLinearAccelerationToInternalLinearAcceleration(WORD acceleration);
00180                 WORD            internalLinearAccelerationToMxlLinearAcceleration(double acceleration);
00181                 double          mxlTorqueToInternalTorque(WORD torque);
00182                 WORD            internalTorqueToMxlTorque(double torque);
00183                 WORD            internalFreqToMxlFreq(double frequency);
00184                 WORD            internalAmplitudeToMxlAmplitude(double amplitude);
00185                 WORD            internalPhaseToMxlPhase(double phase);
00186                 double          mxlPWMToInternalPWM(WORD current);
00187                 WORD            internalPWMToMxlPWM(double current);
00188                 int             clipToMaxWord(int value);
00189                 int             clipToMaxDWord(int value);
00190 
00191         public:
00192 
00193                 C3mxl();
00194                 virtual                 ~C3mxl();
00195                 
00196                 static const char*      translateErrorCode(int errorCode);
00197 
00198                 int                     getID() {return mID;}
00199                 virtual void            setConfig(CDxlConfig* config);
00200                 virtual void            setSerialPort(LxSerial* serialPort);
00201                 virtual int             init(bool sendConfigToMotor=true);
00202 
00203                 virtual int             setPos(double pos, bool shouldSyncWrite=false);
00204                 virtual int             setLinearPos(double pos, bool shouldSyncWrite=false);
00205                 virtual int             setPos(double pos, double absSpeed, bool shouldSyncWrite=false);
00206                 virtual int             setLinearPos(double pos, double absSpeed, bool shouldSyncWrite=false);
00207                 virtual int             setPos(double pos, double absSpeed, double acceleration, bool shouldSyncWrite=false);
00208                 virtual int             setLinearPos(double pos, double absSpeed, double acceleration, bool shouldSyncWrite=false);
00209                 virtual int             setSpeed(double speed, bool shouldSyncWrite=false);
00210                 virtual int             setLinearSpeed(double speed, bool shouldSyncWrite=false);
00211                 virtual int             setAcceleration(double acceleration, bool shouldSyncWrite=false);
00212                 virtual int             setLinearAcceleration(double acceleration, bool shouldSyncWrite=false);
00213                 virtual int             setTorque(double torque, bool shouldSyncWrite=false);
00214                 virtual int             setCurrent(double current, bool shouldSyncWrite=false);
00215                 virtual int             setPWM(double pwm, bool shouldSyncWrite=false);
00216                 virtual int             setPosSpeedTorquePPosDPos(double pos, double speed, double torque, int pPos, int dPos, bool shouldSyncWrite=false);
00217                 virtual int             setPIDCurrent(double p, double d, double i, double i_limit, bool shouldSyncWrite=false);
00218                 virtual int             setPIDPosition(double p, double d, double i, double i_limit, bool shouldSyncWrite=false);
00219                 virtual int             setPIDSpeed(double p, double d, double i, double i_limit, bool shouldSyncWrite=false);
00220                 virtual int             setPIDTorque(double p, double d, double i, double i_limit, bool shouldSyncWrite=false);
00221                 virtual int             setPIDEnergy(double p, double d, double i, double i_limit, bool shouldSyncWrite=false);
00222 
00223                 virtual int             getPIDCurrent(double &p, double &d, double &i, double &i_limit);
00224                 virtual int             getPIDPosition(double &p, double &d, double &i, double &i_limit);
00225                 virtual int             getPIDSpeed(double &p, double &d, double &i, double &i_limit);
00226                 virtual int             getPIDTorque(double &p, double &d, double &i, double &i_limit);
00227                 virtual int             getPIDEnergy(double &p, double &d, double &i, double &i_limit);
00228 
00229                 virtual int             setRetlevel(const int returnlevel);
00230                 virtual int             setReturnDelayTime(const int microsecondsReturnDelay);
00231                 virtual int             setBaudRate(const int baudRate);
00232                 virtual int             setInitialTorqueLimit(double absMaxTorque);
00233                 virtual int             setTorqueLimit(double absMaxTorque);
00234                 virtual int             setAngleLimits(double lowerLimit, double upperLimit);
00235                 virtual int             setAngleLowerLimit(double limit);
00236                 virtual int             setAngleUpperLimit(double limit);
00237                 virtual int             changeID(const int newID);
00238 
00239                 virtual int             set3MxlMode(BYTE mxlMode, bool shouldSyncWrite = false);
00240                 virtual void            setPositiveDirection(bool clockwiseIsPositive);
00241                 virtual int             setPositiveDirectionMotor(bool clockwiseIsPositive);
00242                 virtual int             setPositiveDirectionJoint(bool clockwiseIsPositive);
00243                 virtual int             setMotorOffset(double offset);
00244                 virtual int             setJointOffset(double offset);
00245                 virtual int             setEncoderCountMotor(WORD encodercount);
00246                 virtual int             setEncoderCountJoint(WORD encodercount);
00247                 virtual int             setMotorConstant(WORD motorconstant);
00248                 virtual int             setMaxPeakMotorCurrent(double current);
00249                 virtual int             setMaxContinuousMotorCurrent(double current);
00250                 virtual int             setMotorWindingTimeConstant(double time);
00251                 virtual int             setEncoderIndexLevelMotor(BYTE level);
00252                 virtual int             setWheelDiameter(double diameter);              
00253                 virtual int             setGearboxRatioMotor(float gearboxratio);
00254                 virtual int             setGearboxRatioJoint(float gearboxratio);
00255                 virtual int             setWatchdogMode(BYTE watchdogmode);
00256                 virtual int             setWatchdogTime(BYTE watchdogtime);
00257                 virtual int             setWatchdogMultiplier(BYTE watchdogmultiplier);;
00258                 virtual int             setZeroLengthSpring(double parameterInRad);
00259                 virtual int             setSpringStiffness(double stiffness);
00260                 virtual int             setReferenceEnergy(double energy);
00261                 virtual int             setSineFrequency(double frequency, bool shouldSyncWrite=false);
00262                 virtual int             setSineAmplitude(double amplitude, bool shouldSyncWrite=false);
00263                 virtual int             setSinePhase(double phase, bool shouldSyncWrite=false);
00264                 virtual int             setLogInterval(BYTE interval);
00265                 virtual int             setSyncReadIndex(BYTE index);
00266 
00267                 virtual int             getPos();
00268                 virtual int             getLinearPos();
00269                 virtual int             getPosAndSpeed();
00270                 virtual int             getTorquePosSpeed();
00271                 virtual int             getAcceleration();
00272                 virtual int             getLinearAcceleration();
00273                 virtual int             getState();
00274                 virtual int             getVoltage();
00275                 virtual int             getBusVoltage();
00276                 virtual int             getSensorVoltages();
00277                 virtual int             getCurrent();
00278                 virtual int             getTorque();
00279                 virtual int             getStatus();
00280                 virtual int             getLog();
00281                 virtual int             get3MxlMode();
00282 
00283                 virtual double  presentPos()                    {return mPosition;}
00284                 virtual double  presentLinearPos()              {return mLinearPosition;}
00285                 virtual double  presentSpeed()                  {return mSpeed;}
00286                 virtual double  presentAcceleration()           {return mAcceleration;};
00287                 virtual double  presentLinearAcceleration()     {return mLinearAcceleration;};
00288                 virtual double  presentLoad()                   {mLogWarningLn("presentLoadfunction not implemented");return DXL_NOT_INITIALIZED;}
00289                 virtual double  presentVoltage()                {return mVoltage;}
00290                 virtual double  presentBusVoltage()             {return mBusVoltage;}
00291                 virtual double  presentCurrentADCVoltage()      {return mCurrentADCVoltage;}
00292                 virtual double  presentAnalog1Voltage()         {return mAnalog1Voltage;}
00293                 virtual double  presentAnalog2Voltage()         {return mAnalog2Voltage;}
00294                 virtual double  presentAnalog3Voltage()         {return mAnalog3Voltage;}
00295                 virtual double  presentAnalog4Voltage()         {return mAnalog4Voltage;}
00296                 virtual double  presentTemp()                   {mLogWarningLn("presentTemp function not implemented");return DXL_NOT_INITIALIZED;}
00297                 virtual double  presentCurrent()                {return mCurrent;}
00298                 virtual double  presentTorque()                 {return mTorque;}
00299                 virtual int     presentStatus()                 {return (int)mStatus;}
00300                 virtual bool    presentMotorInitState()         {return (bool)mMotorInitialized;}
00301 
00302                 WORD                    presentCWAngleLimit()   {mLogWarningLn("presentCWAngleLimit function not implemented"); return DXL_NOT_INITIALIZED;}
00303                 WORD                    presentCCWAngleLimit()  {mLogWarningLn("presentCCWAngleLimit function not implemented"); return DXL_NOT_INITIALIZED;}
00304                 double                  presentAngleLowerLimit(){mLogWarningLn("presentAngleLowerLimit function not implemented"); return DXL_NOT_INITIALIZED;};
00305                 double                  presentAngleUpperLimit(){mLogWarningLn("presentAngleUpperLimit function not implemented"); return DXL_NOT_INITIALIZED;};
00306 
00307                 virtual TMxlLog         presentLog()            {return mMxlLog;};
00308                 virtual BYTE            present3MxlMode()       {return mMxlMode;};
00309 
00310                 int                             printReport(FILE* fOut);
00311     int       interpretControlData(BYTE address, BYTE length, BYTE *data);
00312 
00313 
00314                 //not implemented:
00315 //              virtual void    setNullAngle(double nullAngle);
00316 //              virtual int             setBaudRateIndex(const BYTE baudRateIndex);
00317 //              virtual int             setCompliance(BYTE complianceMargin, BYTE complianceSlope);
00318 //              virtual int             setPunch(WORD punch);
00319 //              virtual int             setAlarmLEDMask(const BYTE mask);
00320 //              virtual int             setOperatingMode(const BYTE mode);
00321 //              virtual int             setAlarmShutdownMask(const BYTE mask);
00322 //              virtual int             enableLED(int state);
00323 //              virtual int             enableTorque(int state);
00324 //              virtual int             setVoltageLimits(double minVoltage, double maxVoltage);
00325 //              virtual int             setTemperatureLimit(const int maxTemp);
00326 //              virtual int             setEndlessTurnMode(bool enabled, bool shouldSyncWrite=false);
00327 //              virtual int             setEndlessTurnTorque(double torqueRatio, bool shouldSyncWrite=false);
00328 //              virtual int             getTemp();
00329 //              virtual int             getAngleLimits();
00330 };
00331 
00332 #endif


threemxl
Author(s):
autogenerated on Thu Jun 6 2019 21:10:52