CDxlGroup.cpp
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 #include <threemxl/platform/hardware/dynamixel/CDxlGroup.h>
00007 #include <threemxl/platform/hardware/dynamixel/dynamixel/Dynamixel.h>
00008 #include <threemxl/platform/hardware/dynamixel/3mxl/3mxl.h>
00009 #include <threemxl/platform/hardware/dynamixel/DxlClassFactory.hpp>
00010 
00011 
00012 //***********************************************************//
00013 //********************* CDynamixelGroup *********************//
00014 //***********************************************************//
00015 
00016 CDxlGroup::CDxlGroup(): CDxlCom(), mLog("CDxlGroup")
00017 {
00018         mSerialPort                     = NULL;
00019         mNumDynamixels          = 0;
00020         mSyncPacket = new CDxlSyncWritePacket();
00021         mLog.setLevel(llCrawl);
00022         mSyncRead = false;
00023 //      mLog.enableConsoleOutput("false");
00024 }
00025 
00026 CDxlGroup::~CDxlGroup()
00027 {
00028         // Destroy all dynamixels
00029         for(int iDxl = 0; iDxl < mNumDynamixels; iDxl++)
00030         {
00031                 if (mDynamixels[mNumDynamixels] != NULL)
00032                 {
00033                         delete mDynamixels[mNumDynamixels];
00034                         mDynamixels[mNumDynamixels] = NULL;
00035                         mLogCrawlLn("deleted dynamixel object " << iDxl << " from serialport group " << getName() );
00036                 }
00037         }
00038         delete mSyncPacket;
00039 }
00040 
00041 void CDxlGroup::setSerialPort(LxSerial* serialport)
00042 {
00043         mSerialPort     = serialport;
00044         mName           = "DxlGroup-" + serialport->get_port_name();
00045 }
00046 
00047 void CDxlGroup::setName(const std::string& name)
00048 {
00049         mName = name;
00050 }
00051 
00052 std::string& CDxlGroup::getName()
00053 {
00054         return mName;
00055 }
00056 
00057 bool CDxlGroup::init()
00058 {
00059         return (initAll() == DXL_SUCCESS);
00060 }
00061 
00062 bool CDxlGroup::deinit()
00063 {
00064         //delete all dynamixel objects in this group.
00065         for(int iDxl = 0; iDxl < mNumDynamixels; iDxl++)
00066         {
00067                 if (mDynamixels[iDxl] != NULL)
00068                 {
00069                         delete mDynamixels[iDxl];
00070                         mDynamixels[iDxl] = NULL;
00071                         mLogCrawlLn("deleted dynamixel object " << iDxl << " from serialport group " << getName() );
00072                 }
00073                 else
00074                 {
00075                         mLogErrorLn("unable to delete dynamixel object "<< iDxl << " from serialport group " << getName() );
00076                 }
00077                 mNumDynamixels =0;
00078         }
00079         return true;
00080 }
00081 
00082 int CDxlGroup::action()
00083 {
00084         CDxlPacket packet(DXL_BROADCAST_ID, INST_ACTION, 0);
00085         packet.setChecksum();
00086         return sendPacket(&packet, false);
00087 }
00088 
00089 int CDxlGroup::addNewDynamixel(CDxlConfig* config)
00090 {
00091         CDxlGeneric* pDxl =  gDxlCreate(config->mDxlTypeStr);
00092         if(pDxl==NULL)
00093         {
00094 #ifdef __DBG__
00095                 printf("Dynamixel with wrong type was not added to group!\n");
00096 #endif
00097                 return DXL_NOT_INITIALIZED;
00098         }
00099         else
00100         {
00101                 mDynamixels[mNumDynamixels] = pDxl;
00102                 mDynamixels[mNumDynamixels]->setConfig(config);
00103                 mDynamixels[mNumDynamixels]->setGroup(this);            //<register this group with servo for groupwrites
00104                 mNumDynamixels++;
00105         }
00106         return 0;
00107 }
00108 
00109 int CDxlGroup::setupSyncReadChain()
00110 {
00111   int result = DXL_SUCCESS;
00112   for (int i=0; i<mNumDynamixels; i++)
00113   {
00114     int newResult = mDynamixels[i]->setSyncReadIndex(i+1);
00115     #ifdef __DBG__
00116     if (newResult != DXL_SUCCESS)
00117       mLogErrorLn("Dynamixel with ID " << mDynamixels[i]->getID() << " returned " << translateErrorCode(newResult) << "(last error = " << mDynamixels[i]->getLastError() << ") while setting up sync read chain" );
00118     #endif
00119 
00120     result |= newResult;
00121   }
00122 
00123   if (result == DXL_SUCCESS )
00124     mSyncRead = true;
00125 
00126   return result;
00127 }
00128 
00133 int CDxlGroup::sendSyncWritePacket()
00134 {
00135         if (mNumDynamixels > 0)
00136         {
00137                 if (mSyncPacket->getID() != 0xFE)
00138                 {
00139                         mLogCrawlLn("did not send empty syncWritePacket...");
00140                         return DXL_SUCCESS;
00141                 }
00142                 mSyncPacket->setChecksum();
00143 //              std::cout << "sending packet: " << mSyncPacket->getStartingAddress() << endl;
00144                 int result = sendPacket(mSyncPacket, false);
00145                 mLogCrawlLn("reset syncWritePacket");
00146                 mSyncPacket->reset();   //clear syncPacket for next syncwrite
00147                 return result;
00148         }
00149         else
00150                 return DXL_SUCCESS;
00151 
00152         // No status packet(s) will be returned since a SYNC WRITE uses the broadcast ID
00153 }
00154 
00161 int     CDxlGroup::writeData(int ID, int startingAddress, int dataLength, BYTE* data)
00162 {
00163            if (mSyncPacket->getStartingAddress() == 0) //we are starting a new syncwrite
00164            {
00165                    mLogCrawlLn("configuring syncWritePacket for "<< mNumDynamixels <<" servo's with starting address "<<startingAddress << " and dataLength "<< dataLength );
00166                    mSyncPacket->configurePacket(mNumDynamixels, startingAddress, dataLength);
00167            }
00168            else // Make sure that consecutive servo's are building the same packet
00169            {
00170               if(mSyncPacket->getStartingAddress() != startingAddress)
00171               {
00172                   mLogErrorLn("Trying to write different messages in same syncWritePacket "
00173                                   << "expecting:"
00174                                   << mSyncPacket->getStartingAddress()
00175                                   << ", getting:"
00176                                   << startingAddress);
00177                   return DXL_INVALID_PARAMETER;
00178               }
00179            }
00180            // Now add ID and data at the end of the writing position in the sync write packet
00181            mSyncPacket->addCommand(ID, data, dataLength);
00182            return DXL_SUCCESS;
00183 }
00184 
00185 int CDxlGroup::syncRead(int startingAddress, int dataLength)
00186 {
00187   // Construct sync read packet
00188   CDxlPacket packet(DXL_BROADCAST_ID, INST_READ, 2);
00189   packet.setParam(0, startingAddress);
00190   packet.setParam(1, dataLength);
00191   packet.setChecksum();
00192 
00193   // Send over bus
00194   if (sendPacket(&packet) != DXL_SUCCESS)
00195   {
00196     mLogErrorLn("Couldn't send sync read packet " << packet.getPktString());
00197     return DXL_ERROR;
00198   }
00199 
00200   for (int i=0; i<mNumDynamixels; i++)
00201   {
00202     // Read status packets in sequence (only works if chain was properly set up)
00203     CDxlStatusPacket statusPacket(dataLength);
00204     int result = receivePacketWait(&statusPacket);
00205 
00206     if (result != DXL_SUCCESS)
00207     {
00208       mLogErrorLn("Dynamixel with ID " << mDynamixels[i]->getID() << " returned " << translateErrorCode(result) << "(last error = " << mDynamixels[i]->getLastError() << ") during sync read" );
00209       return result;
00210     }
00211 
00212     mDynamixels[i]->interpretControlData(startingAddress, dataLength, statusPacket.data()+DXL_HEADER_LENGTH);
00213   }
00214 
00215   return DXL_SUCCESS;
00216 }
00217 
00221 void CDxlGroup::setSyncWriteType(const BYTE instruction)
00222 {
00223         mSyncPacket->setInstruction(instruction);
00224 }
00225 
00226 int CDxlGroup::initAll()
00227 {
00228         int result = DXL_SUCCESS;
00229         for (int i=0; i<mNumDynamixels; i++)
00230         {
00231                 if(mDynamixels[i] != NULL)
00232                 {
00233                         mDynamixels[i]->setSerialPort(mSerialPort);
00234                         result |= mDynamixels[i]->init();
00235                         if(result != DXL_SUCCESS){
00236                                 mLogErrorLn("Servo " << i << " did not initialize correctly! Error:" << result);
00237                                 break;
00238                         }
00239                 }
00240                 else
00241                 {
00242                         mLogErrorLn("Trying to initialize non-existent servo with nr:" << i);
00243                 }
00244         }
00245 
00246         return result;
00247 }
00248 
00249 int CDxlGroup::getStateAll()
00250 {
00251   if (mSyncRead)
00252   {
00253     return syncRead(M3XL_VOLTAGE_L, 10);
00254   }
00255   else
00256   {
00257     int result = DXL_SUCCESS;
00258     for (int i=0; i<mNumDynamixels; i++)
00259     {
00260       int newResult = mDynamixels[i]->getState();
00261       #ifdef __DBG__
00262       if (newResult != DXL_SUCCESS)
00263         mLogErrorLn("Dynamixel with ID " << mDynamixels[i]->getID() << " returned " << translateErrorCode(newResult) << "(last error = " << mDynamixels[i]->getLastError() << ")!" );
00264       #endif
00265 
00266       result |= newResult;
00267     }
00268 
00269     return result;
00270   }
00271 }
00272 
00273 int CDxlGroup::getStatusAll()
00274 {
00275         int result = DXL_SUCCESS;
00276         for (int i=0; i<mNumDynamixels; i++)
00277         {
00278                 int newResult = mDynamixels[i]->getStatus();
00279                 #ifdef __DBG__
00280                 if (newResult != DXL_SUCCESS)
00281                         mLogErrorLn("Dynamixel with ID " << mDynamixels[i]->getID() << " returned " << translateErrorCode(newResult) << "(last error = " << mDynamixels[i]->getLastError() << ")!" );
00282                 #endif
00283 
00284                 result |= newResult;
00285         }
00286 
00287         return result;
00288 }
00289 
00290 int CDxlGroup::getPosAll()
00291 {
00292         int result = DXL_SUCCESS;
00293         for (int i=0; i<mNumDynamixels; i++)
00294         {
00295                 int newResult = mDynamixels[i]->getPos();
00296                 #ifdef __DBG__
00297                 if (newResult != DXL_SUCCESS)
00298                         mLogErrorLn("Dynamixel with ID " << mDynamixels[i]->getID() << " returned " << translateErrorCode(newResult) << "(last error = " << mDynamixels[i]->getLastError() << ")!" );
00299                 #endif
00300 
00301                 result |= newResult;
00302         }
00303 
00304         return result;
00305 }
00306 
00307 int CDxlGroup::getPosAndSpeedAll()
00308 {
00309         int result = DXL_SUCCESS;
00310         for (int i=0; i<mNumDynamixels; i++)
00311         {
00312                 int newResult = mDynamixels[i]->getPosAndSpeed();
00313                 #ifdef __DBG__
00314                 if (newResult != DXL_SUCCESS)
00315                         mLogErrorLn("Dynamixel with ID " << mDynamixels[i]->getID() << " returned " << translateErrorCode(newResult) << "(last error = " << mDynamixels[i]->getLastError() << ")!" );
00316                 #endif
00317 
00318                 result |= newResult;
00319         }
00320 
00321         return result;
00322 }
00323 
00324 
00325 int CDxlGroup::getPosSpeedTorqueAll()
00326 {
00327         int result = DXL_SUCCESS;
00328         for (int i=0; i<mNumDynamixels; i++)
00329         {
00330                 int newResult = mDynamixels[i]->getTorquePosSpeed();
00331                 #ifdef __DBG__
00332                 if (newResult != DXL_SUCCESS)
00333                         mLogErrorLn("Dynamixel with ID " << mDynamixels[i]->getID() << " returned " << translateErrorCode(newResult) << "(last error = " << mDynamixels[i]->getLastError() << ")!" );
00334                 #endif
00335 
00336                 result |= newResult;
00337         }
00338 
00339         return result;
00340 }
00341 
00342 int CDxlGroup::getTempAll()
00343 {
00344         int result = DXL_SUCCESS;
00345         for (int i=0; i<mNumDynamixels; i++)
00346         {
00347                 int newResult = mDynamixels[i]->getTemp();
00348                 #ifdef __DBG__
00349                 if (newResult != DXL_SUCCESS)
00350                         mLogErrorLn("Dynamixel with ID " << mDynamixels[i]->getID() << " returned " << translateErrorCode(newResult) << "(last error = " << mDynamixels[i]->getLastError() << ")!" );
00351                 #endif
00352 
00353                 result |= newResult;
00354         }
00355 
00356         return result;
00357 }
00358 
00359 int CDxlGroup::setEndlessTurnModeAll(bool enabled)
00360 {
00361         int result = DXL_SUCCESS;
00362         for (int i=0; i<mNumDynamixels; i++)
00363         {
00364                 int newResult = mDynamixels[i]->setEndlessTurnMode(enabled);
00365                 #ifdef __DBG__
00366                 if (newResult != DXL_SUCCESS)
00367                         mLogErrorLn("Dynamixel with ID " << mDynamixels[i]->getID() << " returned " << translateErrorCode(newResult) << "(last error = " << mDynamixels[i]->getLastError() << ")!" );
00368                 #endif
00369 
00370                 result |= newResult;
00371         }
00372 
00373         return result;
00374 }
00375 
00376 int CDxlGroup::enableTorqueAll(int state)
00377 {
00378         int result = DXL_SUCCESS;
00379         for (int i=0; i<mNumDynamixels; i++)
00380         {
00381                 int newResult = mDynamixels[i]->enableTorque(state);
00382                 #ifdef __DBG__
00383                 if (newResult != DXL_SUCCESS)
00384                         mLogErrorLn("Dynamixel with ID " << mDynamixels[i]->getID() << " returned " << translateErrorCode(newResult) << "(last error = " << mDynamixels[i]->getLastError() << ")!" );
00385                 #endif
00386 
00387                 result |= newResult;
00388         }
00389 
00390         return result;
00391 }
00392 
00393 int CDxlGroup::getNumDynamixels()
00394 {
00395         return mNumDynamixels;
00396 }
00397 
00398 CDxlGeneric* CDxlGroup::getDynamixel(const int index)
00399 {
00400         return mDynamixels[index];
00401 }
00402 
00403 
00404 int CDxlGroup::setConfig(CDxlGroupConfig* config)
00405 {
00406         int result = 0;
00407         for (int i=0; i<config->getNumDynamixels(); i++)
00408         {
00409                 result = addNewDynamixel(config->getDynamixelConfig(i));
00410         }
00411         return result;
00412 }


threemxl
Author(s):
autogenerated on Fri Aug 28 2015 13:21:08