MercurySDK
Software development kit for Mercury digital servos
protocol2_packet_handler.h
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright (C) 2021 <Robot Articulation/code@robotarticulation.com>
3 *
4 * Source files modified to support the Mercury range of digital servo motors from Robot Articulation
5 *******************************************************************************/
6 
7 /*******************************************************************************
8 * Copyright 2017 ROBOTIS CO., LTD.
9 *
10 * Licensed under the Apache License, Version 2.0 (the "License");
11 * you may not use this file except in compliance with the License.
12 * You may obtain a copy of the License at
13 *
14 * http://www.apache.org/licenses/LICENSE-2.0
15 *
16 * Unless required by applicable law or agreed to in writing, software
17 * distributed under the License is distributed on an "AS IS" BASIS,
18 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
19 * See the License for the specific language governing permissions and
20 * limitations under the License.
21 *******************************************************************************/
22 
23 /* Original author: Zerom, Leon (RyuWoon Jung) */
24 
25 #ifndef MERCURY_SDK_INCLUDE_MERCURY_SDK_PROTOCOL2PACKETHANDLER_H_
26 #define MERCURY_SDK_INCLUDE_MERCURY_SDK_PROTOCOL2PACKETHANDLER_H_
27 
28 
29 #include "packet_handler.h"
30 
31 namespace mercury
32 {
33 
37 class WINDECLSPEC Protocol2PacketHandler : public PacketHandler
38 {
39  private:
40  static Protocol2PacketHandler *unique_instance_;
41 
43 
44  uint16_t updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size);
45  void addStuffing(uint8_t *packet);
46  void removeStuffing(uint8_t *packet);
47 
48  public:
53  static Protocol2PacketHandler *getInstance() { return unique_instance_; }
54 
56 
61  float getProtocolVersion() { return 2.0; }
62 
68  const char *getTxRxResult (int result);
69 
75  const char *getRxPacketError (uint8_t error);
76 
92  int txPacket (PortHandler *port, uint8_t *txpacket);
93 
113  int rxPacket (PortHandler *port, uint8_t *rxpacket);
114 
129  int txRxPacket (PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0);
130 
140  int ping (PortHandler *port, uint8_t id, uint8_t *error = 0);
141 
158  int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0);
159 
166  int broadcastPing (PortHandler *port, std::vector<uint8_t> &id_list);
167 
177  int action (PortHandler *port, uint8_t id);
178 
190  int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0);
191 
201  int clearMultiTurn (PortHandler *port, uint8_t id, uint8_t *error = 0);
202 
214  int factoryReset (PortHandler *port, uint8_t id, uint8_t option, uint8_t *error = 0);
215 
230  int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length);
231 
242  int readRx (PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error = 0);
243 
261  int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
262 
271  int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address);
272 
282  int read1ByteRx (PortHandler *port, uint8_t id, uint8_t *data, uint8_t *error = 0);
283 
296  int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0);
297 
306  int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address);
307 
317  int read2ByteRx (PortHandler *port, uint8_t id, uint16_t *data, uint8_t *error = 0);
318 
331  int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0);
332 
341  int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address);
342 
352  int read4ByteRx (PortHandler *port, uint8_t id, uint32_t *data, uint8_t *error = 0);
353 
366  int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0);
367 
379  int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data);
380 
394  int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
395 
405  int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data);
406 
418  int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0);
419 
429  int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data);
430 
442  int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0);
443 
453  int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data);
454 
466  int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0);
467 
480  int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data);
481 
496  int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
497 
509  int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length);
510 
522  int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length);
523 
534  int synchronise (PortHandler *port, uint8_t id, uint8_t *error);
535 
536 };
537 
538 }
539 
540 
541 #endif /* MERCURY_SDK_INCLUDE_MERCURY_SDK_PROTOCOL2PACKETHANDLER_H_ */
The class that inherits Protocol1PacketHandler class or Protocol2PacketHandler class.
The class for port control that inherits PortHandlerLinux, PortHandlerWindows, PortHandlerMac,...
Definition: port_handler.h:56
The class for control Mercury by using Protocol2.0.
static Protocol2PacketHandler * getInstance()
The function that returns Protocol2PacketHandler instance.
float getProtocolVersion()
The function that returns Protocol version used in Protocol2PacketHandler (2.0)