threemxl: CDxlConfig.cpp Source File

Go to the documentation of this file.
00001 // Dynamixel control code - C++ file
00002 // Copyright (c) 2008 Erik Schuitema, Eelko van Breda
00003 // Delft University of Technology
00004 // www.dbl.tudelft.nl
00005 
00006 
00007 #include <stdlib.h>
00008 
00009 #include <threemxl/platform/hardware/dynamixel/CDxlConfig.h>
00010 #include <threemxl/platform/hardware/dynamixel/CDxlGeneric.h>
00011 
00012 
00013 CDxlConfig::CDxlConfig()
00014 {
00015         mCalibType                              = dxlCtNone;
00016 }
00017 
00018 CDxlConfig::~CDxlConfig()
00019 {
00020 }
00021 
00022 int CDxlConfig::configureDynamixel(CDxlGeneric* dxl)
00023 {
00024         int result=0;
00025         // OR'ing the results is not exactly what we want, but anyway it will be
00026         // a good indication in case something goes wrong.
00027 
00028         // Skip internal ID and serial port; they are used inside CDynamixel only.
00029         if (mReturnDelay.isSet())
00030                 { if( dxl->setReturnDelayTime(mReturnDelay) != DXL_SUCCESS) return DXL_ERROR;}
00031         if (mAngleLowerLimit.isSet() && mAngleUpperLimit.isSet())
00032                 {if( dxl->setAngleLimits(mAngleLowerLimit, mAngleUpperLimit) != DXL_SUCCESS) return DXL_ERROR;}
00033         if (mTorqueLimit.isSet())
00034                 {if( dxl->setTorqueLimit(mTorqueLimit) != DXL_SUCCESS) return DXL_ERROR;}
00035         if (mClockwiseIsPositive.isSet())
00036                 {dxl->setPositiveDirection(mClockwiseIsPositive);}
00037 
00038 //<RobotisDynamixel>
00039         if (mTempLimit.isSet())
00040                 {if( dxl->setTemperatureLimit(mTempLimit) != DXL_SUCCESS) return DXL_ERROR;}
00041         if (mVoltageLowerLimit.isSet() && mVoltageUpperLimit.isSet())
00042                 {if( dxl->setVoltageLimits(mVoltageLowerLimit, mVoltageUpperLimit) != DXL_SUCCESS) return DXL_ERROR;}
00043         if (mLED.isSet())
00044                 {if( dxl->enableLED(mLED) != DXL_SUCCESS) return DXL_ERROR;}
00045         if (mTorqueLimit.isSet())
00046                 {if( dxl->setInitialTorqueLimit(mTorqueLimit) != DXL_SUCCESS) return DXL_ERROR;}
00047         if (mAlarmLED.isSet())
00048                 {if( dxl->setAlarmLEDMask(mAlarmLED) != DXL_SUCCESS) return DXL_ERROR;}
00049         if (mAlarmShutdown.isSet())
00050                 {if( dxl->setAlarmShutdownMask(mAlarmShutdown) != DXL_SUCCESS) return DXL_ERROR;}
00051         if (mComplianceMargin.isSet() && mComplianceSlope.isSet())
00052                 {if( dxl->setCompliance(mComplianceMargin, mComplianceSlope) != DXL_SUCCESS) return DXL_ERROR;}
00053         if (mPunch.isSet())
00054                 {if( dxl->setPunch(mPunch) != DXL_SUCCESS) return DXL_ERROR;}
00055 //</RobotisDynamixel>
00056 //<3mxl>
00057         if (m3mxlMode.isSet())
00058                 {if( dxl->set3MxlMode(m3mxlMode) != DXL_SUCCESS) return DXL_ERROR;}
00059         if (mWatchdogMode.isSet())
00060                 {if( dxl->setWatchdogMode(mWatchdogMode) != DXL_SUCCESS) return DXL_ERROR;}
00061         if (mWatchdogTime.isSet())
00062                 {if( dxl->setWatchdogTime(mWatchdogTime) != DXL_SUCCESS) return DXL_ERROR;}
00063         if (mWatchdogMult.isSet())
00064                 {if( dxl->setWatchdogMultiplier(mWatchdogMult) != DXL_SUCCESS) return DXL_ERROR;}
00065         if (mStatusReturnLevel.isSet())
00066                 {if( dxl->setRetlevel(mStatusReturnLevel) != DXL_SUCCESS) return DXL_ERROR;}
00067         if (mMotorConstant.isSet())
00068                 {if( dxl->setMotorConstant(mMotorConstant * MOTOR_CONSTANT_MULTIPLIER) != DXL_SUCCESS) return DXL_ERROR;}
00069         if (mGearboxRatioMotor.isSet())
00070                 {if( dxl->setGearboxRatioMotor(mGearboxRatioMotor) != DXL_SUCCESS) return DXL_ERROR;}
00071         if (mGearboxRatioJoint.isSet())
00072                 {if( dxl->setGearboxRatioJoint(mGearboxRatioJoint) != DXL_SUCCESS) return DXL_ERROR;}
00073         if (mEncoderCountMotor.isSet())
00074                 {if( dxl->setEncoderCountMotor(mEncoderCountMotor) != DXL_SUCCESS) return DXL_ERROR;}
00075         if (mOffsetMotor.isSet())
00076                 {if( dxl->setMotorOffset(mOffsetMotor) != DXL_SUCCESS) return DXL_ERROR;}
00077         if (mMaxUninitialisedMotorCurrent.isSet())
00078                 {if( dxl->setMaxUninitializedMotorCurrent(mMaxUninitialisedMotorCurrent) != DXL_SUCCESS) return DXL_ERROR;}
00079         if (mMaxMotorCurrent.isSet())
00080                 {if( dxl->setMaxMotorCurrent(mMaxMotorCurrent) != DXL_SUCCESS) return DXL_ERROR;}
00081         if (mEncoderCountJoint.isSet())
00082                 {if( dxl->setEncoderCountJoint(mEncoderCountJoint) != DXL_SUCCESS) return DXL_ERROR;}
00083         if (mOffsetJoint.isSet())
00084                 {if( dxl->setJointOffset(mOffsetJoint) != DXL_SUCCESS) return DXL_ERROR;}
00085         if (mZeroLengthSpring.isSet())
00086                 {if( dxl->setZeroLengthSpring(mZeroLengthSpring) != DXL_SUCCESS) return DXL_ERROR;}
00087         if (mSpringStiffness.isSet())
00088                 {if( dxl->setSpringStiffness(mSpringStiffness) != DXL_SUCCESS) return DXL_ERROR;}
00089         if (mJointClockWiseIsPositive.isSet())
00090                 {if( dxl->setPositiveDirectionJoint(mJointClockWiseIsPositive) != DXL_SUCCESS) return DXL_ERROR;}
00091         if (mAcceleration.isSet())
00092                 {if( dxl->setAcceleration(mAcceleration) != DXL_SUCCESS) return DXL_ERROR;}
00093         if (mMaxPeakMotorCurrent.isSet())
00094                 {if( dxl->setMaxPeakMotorCurrent(mMaxPeakMotorCurrent) != DXL_SUCCESS) return DXL_ERROR;}
00095         if (mMaxContinuousMotorCurrent.isSet())
00096                 {if( dxl->setMaxContinuousMotorCurrent(mMaxContinuousMotorCurrent) != DXL_SUCCESS) return DXL_ERROR;}
00097         if (mMotorWindingTimeConstant.isSet())
00098                 {if( dxl->setMotorWindingTimeConstant(mMotorWindingTimeConstant) != DXL_SUCCESS) return DXL_ERROR;}
00099         if (mEncoderIndexLevelMotor.isSet())
00100                 {if( dxl->setEncoderIndexLevelMotor(mEncoderIndexLevelMotor) != DXL_SUCCESS) return DXL_ERROR;}
00101         if (mWheelDiameter.isSet())
00102                 {if( dxl->setWheelDiameter(mWheelDiameter) != DXL_SUCCESS) return DXL_ERROR;}
00103         if (mPCurrent.isSet())
00104                 {if( dxl->setPIDCurrent(mPCurrent, mDCurrent, mICurrent, mILCurrent) != DXL_SUCCESS) return DXL_ERROR;}
00105         if (mPPosition.isSet())
00106                 {if( dxl->setPIDPosition(mPPosition, mDPosition, mIPosition, mILPosition) != DXL_SUCCESS) return DXL_ERROR;}
00107         if (mPSpeed.isSet())
00108                 {if( dxl->setPIDSpeed(mPSpeed, mDSpeed, mISpeed, mILSpeed) != DXL_SUCCESS) return DXL_ERROR;}
00109         if (mPTorque.isSet())
00110                 {if( dxl->setPIDTorque(mPTorque, mDTorque, mITorque, mILTorque) != DXL_SUCCESS) return DXL_ERROR;}
00111         if (mPEnergy.isSet())
00112                 {if( dxl->setPIDEnergy(mPEnergy, mDEnergy, mIEnergy, mILEnergy) != DXL_SUCCESS) return DXL_ERROR;}
00113 //</3mxl>
00114 
00115         return result;
00116 }
00117 
00118 CDxlConfig* CDxlConfig::setID(const int ID)
00119 {
00120         mID = ID;
00121         return this;
00122 }
00123 
00124 bool CDxlConfig::readConfig(const CConfigSection &configNode)
00125 {
00126         bool configresult = true;
00127         configresult &= configNode.get("ID", &mID);
00128         configresult &= configNode.get("type", &mDxlTypeStr, "undefined");
00129         configresult &= configNode.get("returndelay", &mReturnDelay);
00130         configresult &= configNode.get("anglelowerlimit", &mAngleLowerLimit);
00131         configresult &= configNode.get("angleupperlimit", &mAngleUpperLimit);
00132         configresult &= configNode.get("clockwiseispositive", &mClockwiseIsPositive);
00133         configresult &= configNode.get("torquelimit", &mTorqueLimit);
00134         //<3mxl>
00135         configresult &= configNode.get("mxlmode",&m3mxlMode);
00136         configresult &= configNode.get("watchdogmode",&mWatchdogMode);
00137         configresult &= configNode.get("watchdogtime",&mWatchdogTime);
00138         configresult &= configNode.get("watchdogmultiplier",&mWatchdogMult);
00139         configresult &= configNode.get("statusreturnlevel",&mStatusReturnLevel);
00140         configresult &= configNode.get("motorconstant",&mMotorConstant);
00141         configresult &= configNode.get("gearboxratiomotor",&mGearboxRatioMotor);
00142         configresult &= configNode.get("gearboxratiojoint",&mGearboxRatioJoint);
00143         configresult &= configNode.get("encodercountmotor",&mEncoderCountMotor);
00144         configresult &= configNode.get("offsetmotor",&mOffsetMotor);
00145         configresult &= configNode.get("maxuninitialisedmotorcurrent",&mMaxUninitialisedMotorCurrent);
00146         configresult &= configNode.get("maxmotorcurrent",&mMaxMotorCurrent);
00147         configresult &= configNode.get("encodercountjoint",&mEncoderCountJoint);
00148         configresult &= configNode.get("offsetjoint",&mOffsetJoint);
00149 //      configresult &= configNode.get("minjointangle",&mMinJointAngle); we already have mAngleLowerLimit && mAngleUpperLimit
00150 //      configresult &= configNode.get("maxjointangle",&mMaxJointAngle);
00151         configresult &= configNode.get("zerolengthspring",&mZeroLengthSpring);
00152         configresult &= configNode.get("springstiffness",&mSpringStiffness);
00153         configresult &= configNode.get("jointclockwiseispositive",&mJointClockWiseIsPositive);
00154         configresult &= configNode.get("acceleration",&mAcceleration);
00155 
00156         configresult &= configNode.get("maxpeakmotorcurrent",&mMaxPeakMotorCurrent);
00157         configresult &= configNode.get("maxcontinuousmotorcurrent",&mMaxContinuousMotorCurrent);
00158         configresult &= configNode.get("motorwindingtimeconstant",&mMotorWindingTimeConstant);
00159         configresult &= configNode.get("encoderindexlevelmotor",&mEncoderIndexLevelMotor);
00160         configresult &= configNode.get("wheeldiameter",&mWheelDiameter);
00161         
00162         configresult &= configNode.get("pcurrent",&mPCurrent);
00163         configresult &= configNode.get("icurrent",&mICurrent);
00164         configresult &= configNode.get("dcurrent",&mDCurrent);
00165         configresult &= configNode.get("ilcurrent",&mILCurrent);
00166 
00167         configresult &= configNode.get("pposition",&mPPosition);
00168         configresult &= configNode.get("iposition",&mIPosition);
00169         configresult &= configNode.get("dposition",&mDPosition);
00170         configresult &= configNode.get("ilposition",&mILPosition);
00171         
00172         configresult &= configNode.get("pspeed",&mPSpeed);
00173         configresult &= configNode.get("ispeed",&mISpeed);
00174         configresult &= configNode.get("dspeed",&mDSpeed);
00175         configresult &= configNode.get("ilspeed",&mILSpeed);
00176 
00177         configresult &= configNode.get("ptorque",&mPTorque);
00178         configresult &= configNode.get("itorque",&mITorque);
00179         configresult &= configNode.get("dtorque",&mDTorque);
00180         configresult &= configNode.get("iltorque",&mILTorque);
00181 
00182         configresult &= configNode.get("penergy",&mPEnergy);
00183         configresult &= configNode.get("ienergy",&mIEnergy);
00184         configresult &= configNode.get("denergy",&mDEnergy);
00185         configresult &= configNode.get("ilenergy",&mILEnergy);
00186         
00187         //</3mxl>
00188         //<RobotisDynamixel>
00189         configresult &= configNode.get("templimit", &mTempLimit);
00190         configresult &= configNode.get("voltagelowerlimit", &mVoltageLowerLimit);
00191         configresult &= configNode.get("voltageupperlimit", &mVoltageUpperLimit);
00192         configresult &= configNode.get("led", &mLED);
00193         configresult &= configNode.get("alarmled", &mAlarmLED);
00194         configresult &= configNode.get("alarmshutdown", &mAlarmShutdown);
00195         configresult &= configNode.get("compliancemargin", &mComplianceMargin);
00196         configresult &= configNode.get("complianceslope", &mComplianceSlope);
00197         configresult &= configNode.get("punch", &mPunch);
00198         configresult &= configNode.get("nullangle", &mNullAngle);
00199         // Read calibration data: this is a '|'-separated string with DXLCONFIG_NUM_CALIBPOINTS values,
00200         // produced by the manual calibration device (large protractor plate).
00201         std::string calibDataStr;
00202         if (configNode.get("calibdata", &calibDataStr))
00203         {
00204                 if (!calibDataStr.empty())
00205                 {
00206                         int calibDataIndex = 0;
00207                         std::istringstream calibStream(calibDataStr);
00208                         std::string dataFieldStr;
00209 
00210                         while (std::getline(calibStream, dataFieldStr, '|'))
00211                         {
00212                                 if (calibDataIndex >= DXLCONFIG_NUM_CALIBPOINTS)
00213                                 {
00214                                         printf("[ERROR] in CDxlConfig::readConfig() (Dxl ID = %d): more than %d calibration data points found!\n", (int)mID, DXLCONFIG_NUM_CALIBPOINTS);
00215                                         configresult = false;
00216                                         break;
00217                                 }
00218                                 mCalibData[calibDataIndex++] = atof(dataFieldStr.c_str()) + 1.5;        // Add 1.5 to compensate the slack in the calibration measuring method
00219                         }
00220                         if (calibDataIndex != DXLCONFIG_NUM_CALIBPOINTS)
00221                         {
00222                                 printf("[ERROR] in CDxlConfig::readConfig() (Dxl ID = %d): less than %d calibration data points found!\n", (int)mID, DXLCONFIG_NUM_CALIBPOINTS);
00223                                 configresult = false;
00224                         }
00225                         else
00226                                 mCalibType = dxlCtManual;
00227                 }
00228         }
00229 
00230         // Read direct angle LUT
00231         if (configNode.get("anglelut", &calibDataStr))
00232         {
00233                 if (!calibDataStr.empty())
00234                 {
00235                         int calibDataIndex = 0;
00236                         std::istringstream calibStream(calibDataStr);
00237                         std::string dataFieldStr;
00238 
00239                         while (std::getline(calibStream, dataFieldStr, '|'))
00240                         {
00241                                 if (calibDataIndex >= DXL_NUM_POSITIONS)
00242                                 {
00243                                         printf("[ERROR] in CDxlConfig::readConfig() (Dxl ID = %d): more than %d calibration data points found!\n", (int)mID, DXL_NUM_POSITIONS);
00244                                         configresult = false;
00245                                         break;
00246                                 }
00247                                 mAngleLUT[calibDataIndex++] = atof(dataFieldStr.c_str());
00248                         }
00249                         if (calibDataIndex != DXL_NUM_POSITIONS)
00250                         {
00251                                 printf("[ERROR] in CDxlConfig::readConfig() (Dxl ID = %d): less than %d calibration data points found!\n", (int)mID, DXL_NUM_POSITIONS);
00252                                 configresult = false;
00253                         }
00254                         else
00255                         {
00256                                 //printf("[DXLCONFIG] Successfully read anglelut!\n");
00257                                 mCalibType = dxlCtAuto;
00258                         }
00259                 }
00260         }
00261 
00262         //</RobotisDynamixel>
00263 
00264         return configresult;
00265 }