CDxlPacket.hpp
Go to the documentation of this file.
00001 // Dynamixel control code - header file
00002 // Copyright (c) 2008 Erik Schuitema, Eelko van Breda
00003 // Delft University of Technology
00004 // www.dbl.tudelft.nl
00005 
00006 #ifndef __DYNAMIXELPACKET_HPP_INCLUDED__
00007 #define __DYNAMIXELPACKET_HPP_INCLUDED__
00008 
00009 #include <stdio.h>
00010 #include <string.h>
00011 #include <ios>
00012 #include <iostream>
00013 #include <sstream>
00014 
00015 #include "Byte.h"
00016 
00017 
00018 //Packet defines
00019 #define DXL_PKT_MAX_LENGTH                              255
00020 #define DXL_HEADER_LENGTH                               5 // [0xFF] [0xFF] [ID] [LENGTH] [INSTRUCTION]
00021 #define DXL_CRC_LENGTH                                  1
00022 #define DXL_MAX_NUM_PARAMS                              (DXL_PKT_MAX_LENGTH - DXL_HEADER_LENGTH - DXL_CRC_LENGTH)
00023 
00024 // Broadcast ID
00025 #define DXL_BROADCAST_ID                        0xFE
00026 
00027 
00028 
00029 class CDxlPacket
00030 {
00031         protected:
00032                 unsigned int    mLength;
00033                 unsigned int    mNumParams;
00034                 bool                    mInit;
00035                 BYTE                    mData[DXL_PKT_MAX_LENGTH];
00036 
00037         public:
00038                 CDxlPacket()    {mInit = false;}        // Warning; creates uninitialized packet!!
00039 
00040                 CDxlPacket(const int ID, const BYTE instruction, const int numParams)
00041                 {
00042                         mData[0]        = 0xFF;
00043                         mData[1]        = 0xFF;
00044                         mData[2]        = ID;
00045                         mData[3]        = numParams + 2;        // length of the packet according to dynamixel definition
00046                         mData[4]        = instruction;
00047                         mLength         = numParams + DXL_HEADER_LENGTH + DXL_CRC_LENGTH;       // true size of the packet: numParams + header + checksum
00048 
00049                         mNumParams      = numParams;
00050                         mInit = true;
00051                 }
00052 
00053                 void reset()
00054                 {
00055                         mData[0]        = 0xFF;
00056                         mData[1]        = 0xFF;
00057                         mData[2]        = 0;
00058                         mData[3]        = 2;    // length of the packet according to dynamixel definition
00059                         mData[4]        = 0;
00060                         mLength         = DXL_HEADER_LENGTH + DXL_CRC_LENGTH;   // true size of the packet: numParams + header + checksum
00061 
00062                         mNumParams      = 0;
00063                         mInit = true;
00064                 }
00065 
00066                 inline void             setID(const int ID)                                     {mData[2]       = ID;}
00067                 inline BYTE             length()                                                        {return mLength;}
00068                 inline BYTE*    data()                                                          {return mData;}
00069                 inline BYTE             getError()                                                      {return mData[4];}
00070                 inline BYTE             getID()                                                         {return mData[2];}
00071 
00072                 // adjustLength() allows to adjust the length fields (2!) of the packet without calling new/delete
00073                 inline void             adjustLength(const int numParams)
00074                 {
00075                         mData[3]        = numParams + 2;
00076                         mLength         = numParams + DXL_HEADER_LENGTH + DXL_CRC_LENGTH;
00077                         mNumParams      = numParams;
00078 //                      printf("Number of params = %d \n",mNumParams);
00079                 }
00080 
00081                 // Checksum functionality
00082                 inline BYTE             readChecksum()                                          {return mData[mLength-1];}
00083                 inline void             setChecksum()                                           {mData[mLength-1] = calcChecksum();}
00084                 inline BYTE             calcChecksum()
00085                 {
00086                         BYTE result=0;
00087                         for (unsigned int i=2; i<mLength-1; i++)
00088                                 result += mData[i];
00089 
00090                         return ~result;
00091                 }
00092 
00093                 // Interface for the parameters of the packet
00094                 inline BYTE             getParam(const BYTE paramIndex)                                                                         {return mData[DXL_HEADER_LENGTH + paramIndex];}
00095                 inline void             getParams(const BYTE paramIndex, const BYTE numParams, BYTE* data)      {memcpy(data, mData + DXL_HEADER_LENGTH + paramIndex, numParams);}
00096                 inline void             setParam(const BYTE paramIndex, const BYTE value)                                       {mData[DXL_HEADER_LENGTH + paramIndex] = value;}
00097                 inline void             setParams(const BYTE paramIndex, const BYTE numParams, BYTE* data)      {memcpy(mData + DXL_HEADER_LENGTH + paramIndex, data, numParams);}
00098                 inline void     setInstruction(const BYTE instruction)                                                          {mData[4]       = instruction;}
00099                 inline void             clearParams()                                                                                                           {memset(mData + DXL_HEADER_LENGTH, 0, mNumParams);}
00100 
00101                 // getParamWord and setParamWord: paramIndex is always counted in BYTES!
00102                 inline WORD             getParamWord(const BYTE paramIndex)                                                                     {return *(WORD*)(mData + DXL_HEADER_LENGTH + paramIndex);}
00103                 inline void             setParamWord(const BYTE paramIndex, const WORD value)                           {*(WORD*)(mData + DXL_HEADER_LENGTH + paramIndex) = value;}
00104 
00105 
00106                 std::string                     getPktString()
00107                 {
00108                         return getPktString(mLength);
00109                 }
00110                 std::string                     getPktString(unsigned char length)
00111                 {
00112                         if(mInit)
00113                         {
00114                                 using namespace std;
00115                                 stringstream pktString;
00116                                 for (unsigned int i = 0; i < length; i++)
00117                                 {
00118                                         pktString << hex << (unsigned int)mData[i] << " ";
00119                                 }
00120                                 return pktString.str();
00121                         }
00122                         else
00123                                 return "";
00124                 }
00125 
00126 };
00127 
00128 
00129 class CDxlStatusPacket: public CDxlPacket
00130 {
00131         public:
00132                 CDxlStatusPacket(const int numParams):
00133                         CDxlPacket()
00134                 {
00135                         mLength = numParams + DXL_HEADER_LENGTH + DXL_CRC_LENGTH;
00136                         mNumParams      = numParams;
00137                         memset(mData, 0, mLength);
00138                         mInit = true;
00139                 }
00140 };
00141 
00142 class CDxlSyncWritePacket: public CDxlPacket
00143 {
00144         protected:
00145         int                     mStartingAddress;
00146         int                     mDxlPointer;
00147         int                             mNumParamsPerDxl;
00148 
00149                 // adjustLengthSyncWrite() adjusts the packet's size for use with the SYNC_WRITE packet (multiple dxl's).
00150                 inline void             adjustLengthSyncWrite(const int numParamsPerDynamixel, const int numDynamixels)
00151                 {
00152                         // numRegularParameters = (numParameters + sizeof(ID))*numDynamixels + [start address] + [length of data]
00153                         adjustLength((numParamsPerDynamixel+1)*numDynamixels + 2);
00154                 }
00155 
00156         public:
00157 
00158                 CDxlSyncWritePacket():
00159                         CDxlPacket(DXL_BROADCAST_ID, 0x83, 0)//INST_SYNC_WRITE
00160                 {
00161                         mNumParamsPerDxl = 0;
00162                         mStartingAddress = 0;
00163                         mDxlPointer = 2;
00164                 }
00165 
00166                 // instruction should be either INST_SYNC_WRITE or INST_SYNC_REG_WRITE
00167                 // Make sure you call configurePacket() before using the packet.
00168                 CDxlSyncWritePacket(const BYTE instruction):
00169                         CDxlPacket(DXL_BROADCAST_ID, instruction, 0)
00170                 {
00171                         mNumParamsPerDxl = 0;
00172                         mStartingAddress = 0;
00173                         mDxlPointer = 2;
00174                 }
00175 
00182                 void    configurePacket(const int numDynamixels, const BYTE startRegister, const int numParamsPerDynamixel)
00183                 {
00184                         setID(DXL_BROADCAST_ID);
00185                         // Adjust the length of our mSyncPacket and clear it
00186                         adjustLengthSyncWrite(numParamsPerDynamixel, numDynamixels); // Resizes the packet for the number of added dynamixels and the desired parameters settings
00187                         clearParams();
00188                         // Set the first two parameters
00189                         setParam(0, startRegister);
00190                         setParam(1, numParamsPerDynamixel);
00191                         setInstruction(0x83);
00192                         mStartingAddress = startRegister;
00193                         mNumParamsPerDxl = numParamsPerDynamixel;
00194                 }
00195 
00196 //              // this function retrieves the location inside the packet where a dynamixel can write its data (ID + params)
00197 //              inline BYTE*    getSyncWriteLocation(const int dynamixelIndex)
00198 //              {
00199 //                      // The first parameter in a SYNC_WRITE packet is located at mData + DXL_HEADER_LENGTH
00200 //                      // See adjustLengthSyncWrite() for further explanation
00201 //                      return mData + DXL_HEADER_LENGTH + 2 + dynamixelIndex*(mNumParamsPerDxl+1);
00202 //              }
00203 
00207                 void addCommand(const int ID, BYTE* data, int dataLength)
00208                 {
00209                         setParam(mDxlPointer,ID);
00210                         mDxlPointer++;
00211                         setParams(mDxlPointer, dataLength, data);
00212                         mDxlPointer += dataLength;
00213                 }
00214 
00218                 void reset()
00219                 {
00220                         CDxlPacket::reset();
00221                         mNumParamsPerDxl = 0;
00222                         mStartingAddress = 0;
00223                         mDxlPointer = 2;
00224                 }
00225 
00230                 int getStartingAddress() {return mStartingAddress;}
00231 
00232 };
00233 
00234 
00235 
00236 #endif // __DYNAMIXELPACKET_HPP_INCLUDED__


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