Class PacketHandler

Inheritance Relationships

Derived Types

Class Documentation

class dynamixel::PacketHandler

The class that inherits Protocol1PacketHandler class or Protocol2PacketHandler class.

Subclassed by dynamixel::Protocol1PacketHandler, dynamixel::Protocol2PacketHandler

Public Functions

inline virtual ~PacketHandler()
virtual float getProtocolVersion() = 0

The function that returns Protocol version.

Returns

protocol version

virtual const char *getTxRxResult(int result) = 0

The function that gets description of communication result.

Parameters

result – Communication result which might be gotten by the tx rx functions

Returns

description of communication result in const char* (string)

virtual const char *getRxPacketError(uint8_t error) = 0

The function that gets description of hardware error.

Parameters

error – Dynamixel hardware error which might be gotten by the tx rx functions

Returns

description of hardware error in const char* (string)

virtual int txPacket(PortHandler *port, uint8_t *txpacket) = 0

The function that transmits the instruction packet txpacket via PortHandler port. @description The function clears the port buffer by PortHandler::clearPort() function, @description then transmits txpacket by PortHandler::writePort() function. @description The function activates only when the port is not busy and when the packet is already written on the port buffer.

Parameters
  • portPortHandler instance

  • txpacket – packet for transmission

Returns

COMM_PORT_BUSY

Returns

when the port is already in use

Returns

COMM_TX_ERROR

Returns

when txpacket is out of range described by TXPACKET_MAX_LEN

Returns

COMM_TX_FAIL

Returns

when written packet is shorter than expected

Returns

or COMM_SUCCESS

virtual int rxPacket(PortHandler *port, uint8_t *rxpacket) = 0

The function that receives packet (rxpacket) during designated time via PortHandler port @description The function repeatedly tries to receive rxpacket by PortHandler::readPort() function. @description It breaks out @description when PortHandler::isPacketTimeout() shows the timeout, @description when rxpacket seemed as corrupted, or @description when nothing received.

Parameters
  • portPortHandler instance

  • rxpacket – received packet

Returns

COMM_RX_CORRUPT

Returns

when it received the packet but it couldn’t find header in the packet

Returns

when it found header in the packet but the id, length or error value is out of range

Returns

when it received the packet but it is shorted than expected

Returns

COMM_RX_TIMEOUT

Returns

when there is no rxpacket received until PortHandler::isPacketTimeout() shows the timeout

Returns

COMM_SUCCESS

Returns

when rxpacket passes checksum test

Returns

or COMM_RX_FAIL

virtual int txRxPacket(PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0) = 0

The function that transmits packet (txpacket) and receives packet (rxpacket) during designated time via PortHandler port @description The function calls PacketHandler::txPacket(), @description and calls PacketHandler::rxPacket() if it succeeds PacketHandler::txPacket(). @description It breaks out @description when it fails PacketHandler::txPacket(), @description when txpacket is called by PacketHandler::broadcastPing() / PacketHandler::syncWriteTxOnly() / PacketHandler::regWriteTxOnly / PacketHandler::action.

Parameters
  • portPortHandler instance

  • txpacket – packet for transmission

  • rxpacket – received packet

Returns

COMM_SUCCESS

Returns

when it succeeds PacketHandler::txPacket() and PacketHandler::rxPacket()

Returns

or the other communication results which come from PacketHandler::txPacket() and PacketHandler::rxPacket()

virtual int ping(PortHandler *port, uint8_t id, uint8_t *error = 0) = 0

The function that pings Dynamixel but doesn’t take its model number @description The function calls PacketHandler::ping() which gets Dynamixel model number, @description but doesn’t carry the model number.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • error – Dynamixel hardware error

Returns

communication results which come from PacketHandler::ping()

virtual int ping(PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0) = 0

The function that pings Dynamixel and takes its model number @description The function makes an instruction packet with INST_PING, @description transmits the packet with PacketHandler::txRxPacket(), @description and call PacketHandler::readTxRx to read model_number in the rx buffer. @description It breaks out @description when it tries to transmit to BROADCAST_ID.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • error – Dynamixel hardware error

Returns

COMM_NOT_AVAILABLE

Returns

when it tries to transmit to BROADCAST_ID

Returns

COMM_SUCCESS

Returns

when it succeeds to ping Dynamixel and get model_number from it

Returns

or the other communication results which come from PacketHandler::txRxPacket() and PacketHandler::readTxRx()

virtual int broadcastPing(PortHandler *port, std::vector<uint8_t> &id_list) = 0

(Available only in Protocol 2.0) The function that pings all connected Dynamixel

Parameters
  • portPortHandler instance

  • id_list – ID list of Dynamixels which are found by broadcast ping

Returns

COMM_NOT_AVAILABLE

virtual int action(PortHandler *port, uint8_t id) = 0

The function that makes Dynamixels run as written in the Dynamixel register @description The function makes an instruction packet with INST_ACTION, @description transmits the packet with PacketHandler::txRxPacket(). @description To use this function, Dynamixel register should be set by PacketHandler::regWriteTxOnly() or PacketHandler::regWriteTxRx()

Parameters
Returns

communication results which come from PacketHandler::txRxPacket()

virtual int reboot(PortHandler *port, uint8_t id, uint8_t *error = 0) = 0

The function that makes Dynamixel reboot @description The function makes an instruction packet with INST_REBOOT, @description transmits the packet with PacketHandler::txRxPacket(), @description then Dynamixel reboots. @description During reboot, its LED will blink.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • error – Dynamixel hardware error

Returns

COMM_NOT_AVAILABLE

virtual int clearMultiTurn(PortHandler *port, uint8_t id, uint8_t *error = 0) = 0

The function that reset multi-turn revolution information of Dynamixel @description The function makes an instruction packet with INST_CLEAR, @description transmits the packet with PacketHandler::txRxPacket(). @description Applied Products : MX with Protocol 2.0 (Firmware v42 or above), @description Dynamixel X-series (Firmware v42 or above).

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • error – Dynamixel hardware error

Returns

communication results which come from PacketHandler::txRxPacket()

virtual int factoryReset(PortHandler *port, uint8_t id, uint8_t option = 0, uint8_t *error = 0) = 0

The function that makes Dynamixel reset as it was produced in the factory @description The function makes an instruction packet with INST_FACTORY_RESET, @description transmits the packet with PacketHandler::txRxPacket(). @description Be careful of the use.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • option – Reset option

  • error – Dynamixel hardware error

Returns

communication results which come from PacketHandler::txRxPacket()

virtual int readTx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length) = 0

The function that transmits INST_READ instruction packet @description The function makes an instruction packet with INST_READ, @description transmits the packet with PacketHandler::txPacket(). @description It breaks out @description when it tries to transmit to BROADCAST_ID.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • address – Address of the data for read

  • length – Length of the data for read

Returns

COMM_NOT_AVAILABLE

Returns

when it tries to transmit to BROADCAST_ID

Returns

or the other communication results which come from PacketHandler::txPacket()

virtual int readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0

The function that receives the packet and reads the data in the packet @description The function receives the packet which might be come by previous INST_READ instruction packet transmission, @description gets the data from the packet.

Parameters
  • portPortHandler instance

  • length – Length of the data for read

  • data – Data extracted from the packet

  • error – Dynamixel hardware error

Returns

communication results which come from PacketHandler::rxPacket()

virtual int readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0

The function that transmits INST_READ instruction packet, and read data from received packet @description The function makes an instruction packet with INST_READ, @description transmits and receives the packet with PacketHandler::txRxPacket(), @description gets the data from the packet. @description It breaks out @description when it tries to transmit to BROADCAST_ID.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • address – Address of the data for read

  • length – Length of the data for read

  • data – Data extracted from the packet

  • error – Dynamixel hardware error

Returns

COMM_NOT_AVAILABLE

Returns

when it tries to transmit to BROADCAST_ID

Returns

or the other communication results which come from PacketHandler::txRxPacket()

virtual int read1ByteTx(PortHandler *port, uint8_t id, uint16_t address) = 0

The function that calls PacketHandler::readTx() function for reading 1 byte data @description The function calls PacketHandler::readTx() function for reading 1 byte data.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • address – Address of the data for read

Returns

communication results which come from PacketHandler::readTx()

virtual int read1ByteRx(PortHandler *port, uint8_t id, uint8_t *data, uint8_t *error = 0) = 0

The function that calls PacketHandler::readRx() function and reads 1 byte data on the packet @description The function calls PacketHandler::readRx() function, @description gets 1 byte data from the packet.

Parameters
  • portPortHandler instance

  • data – Data extracted from the packet

  • error – Dynamixel hardware error

Returns

communication results which come from PacketHandler::readRx()

virtual int read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0) = 0

The function that calls PacketHandler::readTxRx() function for reading 1 byte data @description The function calls PacketHandler::readTxRx(), @description gets 1 byte data from the packet.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • address – Address of the data for read

  • length – Length of the data for read

  • data – Data extracted from the packet

  • error – Dynamixel hardware error

Returns

communication results which come from PacketHandler::txRxPacket()

virtual int read2ByteTx(PortHandler *port, uint8_t id, uint16_t address) = 0

The function that calls PacketHandler::readTx() function for reading 2 byte data @description The function calls PacketHandler::readTx() function for reading 2 byte data.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • address – Address of the data for read

Returns

communication results which come from PacketHandler::readTx()

virtual int read2ByteRx(PortHandler *port, uint8_t id, uint16_t *data, uint8_t *error = 0) = 0

The function that calls PacketHandler::readRx() function and reads 2 byte data on the packet @description The function calls PacketHandler::readRx() function, @description gets 2 byte data from the packet.

Parameters
  • portPortHandler instance

  • data – Data extracted from the packet

  • error – Dynamixel hardware error

Returns

communication results which come from PacketHandler::readRx()

virtual int read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0) = 0

The function that calls PacketHandler::readTxRx() function for reading 2 byte data @description The function calls PacketHandler::readTxRx(), @description gets 2 byte data from the packet.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • address – Address of the data for read

  • length – Length of the data for read

  • data – Data extracted from the packet

  • error – Dynamixel hardware error

Returns

communication results which come from PacketHandler::txRxPacket()

virtual int read4ByteTx(PortHandler *port, uint8_t id, uint16_t address) = 0

The function that calls PacketHandler::readTx() function for reading 4 byte data @description The function calls PacketHandler::readTx() function for reading 4 byte data.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • address – Address of the data for read

Returns

communication results which come from PacketHandler::readTx()

virtual int read4ByteRx(PortHandler *port, uint8_t id, uint32_t *data, uint8_t *error = 0) = 0

The function that calls PacketHandler::readRx() function and reads 4 byte data on the packet @description The function calls PacketHandler::readRx() function, @description gets 4 byte data from the packet.

Parameters
  • portPortHandler instance

  • data – Data extracted from the packet

  • error – Dynamixel hardware error

Returns

communication results which come from PacketHandler::readRx()

virtual int read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0) = 0

The function that calls PacketHandler::readTxRx() function for reading 4 byte data @description The function calls PacketHandler::readTxRx(), @description gets 4 byte data from the packet.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • address – Address of the data for read

  • length – Length of the data for read

  • data – Data extracted from the packet

  • error – Dynamixel hardware error

Returns

communication results which come from PacketHandler::txRxPacket()

virtual int writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) = 0

The function that transmits INST_WRITE instruction packet with the data for write @description The function makes an instruction packet with INST_WRITE and the data for write, @description transmits the packet with PacketHandler::txPacket().

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • address – Address of the data for write

  • length – Length of the data for write

  • data – Data for write

Returns

communication results which come from PacketHandler::txPacket()

virtual int writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0

The function that transmits INST_WRITE instruction packet with the data for write, and receives the packet @description The function makes an instruction packet with INST_WRITE and the data for write, @description transmits and receives the packet with PacketHandler::txRxPacket(), @description gets the error from the packet.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • address – Address of the data for write

  • length – Length of the data for write

  • data – Data for write

  • error – Dynamixel hardware error

Returns

communication results which come from PacketHandler::txRxPacket()

virtual int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data) = 0

The function that calls PacketHandler::writeTxOnly() for writing 1 byte data @description The function calls PacketHandler::writeTxOnly() for writing 1 byte data.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • address – Address of the data for write

  • data – Data for write

Returns

communication results which come from PacketHandler::writeTxOnly()

virtual int write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0) = 0

The function that calls PacketHandler::writeTxRx() for writing 1 byte data and receives the packet @description The function calls PacketHandler::writeTxRx() for writing 1 byte data and receves the packet, @description gets the error from the packet.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • address – Address of the data for write

  • data – Data for write

  • error – Dynamixel hardware error

Returns

communication results which come from PacketHandler::writeTxRx()

virtual int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data) = 0

The function that calls PacketHandler::writeTxOnly() for writing 2 byte data @description The function calls PacketHandler::writeTxOnly() for writing 2 byte data.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • address – Address of the data for write

  • data – Data for write

Returns

communication results which come from PacketHandler::writeTxOnly()

virtual int write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0) = 0

The function that calls PacketHandler::writeTxRx() for writing 2 byte data and receives the packet @description The function calls PacketHandler::writeTxRx() for writing 2 byte data and receves the packet, @description gets the error from the packet.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • address – Address of the data for write

  • data – Data for write

  • error – Dynamixel hardware error

Returns

communication results which come from PacketHandler::writeTxRx()

virtual int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data) = 0

The function that calls PacketHandler::writeTxOnly() for writing 4 byte data @description The function calls PacketHandler::writeTxOnly() for writing 4 byte data.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • address – Address of the data for write

  • data – Data for write

Returns

communication results which come from PacketHandler::writeTxOnly()

virtual int write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0) = 0

The function that calls PacketHandler::writeTxRx() for writing 4 byte data and receives the packet @description The function calls PacketHandler::writeTxRx() for writing 4 byte data and receves the packet, @description gets the error from the packet.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • address – Address of the data for write

  • data – Data for write

  • error – Dynamixel hardware error

Returns

communication results which come from PacketHandler::writeTxRx()

virtual int regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) = 0

The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, @description transmits the packet with PacketHandler::txPacket(). @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • address – Address of the data for write

  • length – Length of the data for write

  • data – Data for write

Returns

communication results which come from PacketHandler::txPacket()

virtual int regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0

The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register, and receives the packet @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, @description transmits and receives the packet with PacketHandler::txRxPacket(), @description gets the error from the packet. @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel.

Parameters
  • portPortHandler instance

  • id – Dynamixel ID

  • address – Address of the data for write

  • length – Length of the data for write

  • data – Data for write

  • error – Dynamixel hardware error

Returns

communication results which come from PacketHandler::txRxPacket()

virtual int syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) = 0

The function that transmits INST_SYNC_READ instruction packet @description The function makes an instruction packet with INST_SYNC_READ, @description transmits the packet with PacketHandler::txPacket().

Parameters
  • portPortHandler instance

  • start_address – Address of the data for Sync Read

  • data_length – Length of the data for Sync Read

  • param – Parameter for Sync Read

  • param_length – Length of the data for Sync Read

Returns

communication results which come from PacketHandler::txPacket()

virtual int syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) = 0

The function that transmits INST_SYNC_WRITE instruction packet @description The function makes an instruction packet with INST_SYNC_WRITE, @description transmits the packet with PacketHandler::txRxPacket().

Parameters
  • portPortHandler instance

  • start_address – Address of the data for Sync Write

  • data_length – Length of the data for Sync Write

  • param – Parameter for Sync Write

  • param_length – Length of the data for Sync Write

Returns

communication results which come from PacketHandler::txRxPacket()

virtual int bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length) = 0

The function that transmits INST_BULK_READ instruction packet @description The function makes an instruction packet with INST_BULK_READ, @description transmits the packet with PacketHandler::txPacket().

Parameters
  • portPortHandler instance

  • param – Parameter for Bulk Read

  • param_length – Length of the data for Bulk Read

Returns

communication results which come from PacketHandler::txPacket()

virtual int bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length) = 0

The function that transmits INST_BULK_WRITE instruction packet @description The function makes an instruction packet with INST_BULK_WRITE, @description transmits the packet with PacketHandler::txRxPacket().

Parameters
  • portPortHandler instance

  • param – Parameter for Bulk Write

  • param_length – Length of the data for Bulk Write

Returns

communication results which come from PacketHandler::txRxPacket()

Public Static Functions

static PacketHandler *getPacketHandler(float protocol_version = 2.0)

The function that returns PacketHandler instance.

Returns

PacketHandler instance

Protected Functions

inline PacketHandler()