protocol1_packet_handler.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2017 ROBOTIS CO., LTD.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *******************************************************************************/
16 
17 /* Author: zerom, Ryu Woon Jung (Leon) */
18 
19 #if defined(__linux__)
21 #elif defined(__APPLE__)
23 #elif defined(_WIN32) || defined(_WIN64)
24 #define WINDLLEXPORT
26 #elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__)
27 #include "../../include/dynamixel_sdk/protocol1_packet_handler.h"
28 #endif
29 
30 #include <string.h>
31 #include <stdlib.h>
32 
33 #define TXPACKET_MAX_LEN (250)
34 #define RXPACKET_MAX_LEN (250)
35 
37 #define PKT_HEADER0 0
38 #define PKT_HEADER1 1
39 #define PKT_ID 2
40 #define PKT_LENGTH 3
41 #define PKT_INSTRUCTION 4
42 #define PKT_ERROR 4
43 #define PKT_PARAMETER0 5
44 
46 #define ERRBIT_VOLTAGE 1 // Supplied voltage is out of the range (operating volatage set in the control table)
47 #define ERRBIT_ANGLE 2 // Goal position is written out of the range (from CW angle limit to CCW angle limit)
48 #define ERRBIT_OVERHEAT 4 // Temperature is out of the range (operating temperature set in the control table)
49 #define ERRBIT_RANGE 8 // Command(setting value) is out of the range for use.
50 #define ERRBIT_CHECKSUM 16 // Instruction packet checksum is incorrect.
51 #define ERRBIT_OVERLOAD 32 // The current load cannot be controlled by the set torque.
52 #define ERRBIT_INSTRUCTION 64 // Undefined instruction or delivering the action command without the reg_write command.
53 
54 using namespace dynamixel;
55 
57 
59 
61 {
62  switch(result)
63  {
64  case COMM_SUCCESS:
65  return "[TxRxResult] Communication success.";
66 
67  case COMM_PORT_BUSY:
68  return "[TxRxResult] Port is in use!";
69 
70  case COMM_TX_FAIL:
71  return "[TxRxResult] Failed transmit instruction packet!";
72 
73  case COMM_RX_FAIL:
74  return "[TxRxResult] Failed get status packet from device!";
75 
76  case COMM_TX_ERROR:
77  return "[TxRxResult] Incorrect instruction packet!";
78 
79  case COMM_RX_WAITING:
80  return "[TxRxResult] Now recieving status packet!";
81 
82  case COMM_RX_TIMEOUT:
83  return "[TxRxResult] There is no status packet!";
84 
85  case COMM_RX_CORRUPT:
86  return "[TxRxResult] Incorrect status packet!";
87 
88  case COMM_NOT_AVAILABLE:
89  return "[TxRxResult] Protocol does not support This function!";
90 
91  default:
92  return "";
93  }
94 }
95 
96 const char *Protocol1PacketHandler::getRxPacketError(uint8_t error)
97 {
98  if (error & ERRBIT_VOLTAGE)
99  return "[RxPacketError] Input voltage error!";
100 
101  if (error & ERRBIT_ANGLE)
102  return "[RxPacketError] Angle limit error!";
103 
104  if (error & ERRBIT_OVERHEAT)
105  return "[RxPacketError] Overheat error!";
106 
107  if (error & ERRBIT_RANGE)
108  return "[RxPacketError] Out of range error!";
109 
110  if (error & ERRBIT_CHECKSUM)
111  return "[RxPacketError] Checksum error!";
112 
113  if (error & ERRBIT_OVERLOAD)
114  return "[RxPacketError] Overload error!";
115 
116  if (error & ERRBIT_INSTRUCTION)
117  return "[RxPacketError] Instruction code error!";
118 
119  return "";
120 }
121 
122 int Protocol1PacketHandler::txPacket(PortHandler *port, uint8_t *txpacket)
123 {
124  uint8_t checksum = 0;
125  uint8_t total_packet_length = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH
126  uint8_t written_packet_length = 0;
127 
128  if (port->is_using_)
129  return COMM_PORT_BUSY;
130  port->is_using_ = true;
131 
132  // check max packet length
133  if (total_packet_length > TXPACKET_MAX_LEN)
134  {
135  port->is_using_ = false;
136  return COMM_TX_ERROR;
137  }
138 
139  // make packet header
140  txpacket[PKT_HEADER0] = 0xFF;
141  txpacket[PKT_HEADER1] = 0xFF;
142 
143  // add a checksum to the packet
144  for (uint16_t idx = 2; idx < total_packet_length - 1; idx++) // except header, checksum
145  checksum += txpacket[idx];
146  txpacket[total_packet_length - 1] = ~checksum;
147 
148  // tx packet
149  port->clearPort();
150  written_packet_length = port->writePort(txpacket, total_packet_length);
151  if (total_packet_length != written_packet_length)
152  {
153  port->is_using_ = false;
154  return COMM_TX_FAIL;
155  }
156 
157  return COMM_SUCCESS;
158 }
159 
160 int Protocol1PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket)
161 {
162  int result = COMM_TX_FAIL;
163 
164  uint8_t checksum = 0;
165  uint8_t rx_length = 0;
166  uint8_t wait_length = 6; // minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM)
167 
168  while(true)
169  {
170  rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length);
171  if (rx_length >= wait_length)
172  {
173  uint8_t idx = 0;
174 
175  // find packet header
176  for (idx = 0; idx < (rx_length - 1); idx++)
177  {
178  if (rxpacket[idx] == 0xFF && rxpacket[idx+1] == 0xFF)
179  break;
180  }
181 
182  if (idx == 0) // found at the beginning of the packet
183  {
184  if (rxpacket[PKT_ID] > 0xFD || // unavailable ID
185  rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN || // unavailable Length
186  rxpacket[PKT_ERROR] > 0x7F) // unavailable Error
187  {
188  // remove the first byte in the packet
189  for (uint16_t s = 0; s < rx_length - 1; s++)
190  rxpacket[s] = rxpacket[1 + s];
191  //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx);
192  rx_length -= 1;
193  continue;
194  }
195 
196  // re-calculate the exact length of the rx packet
197  if (wait_length != rxpacket[PKT_LENGTH] + PKT_LENGTH + 1)
198  {
199  wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1;
200  continue;
201  }
202 
203  if (rx_length < wait_length)
204  {
205  // check timeout
206  if (port->isPacketTimeout() == true)
207  {
208  if (rx_length == 0)
209  {
210  result = COMM_RX_TIMEOUT;
211  }
212  else
213  {
214  result = COMM_RX_CORRUPT;
215  }
216  break;
217  }
218  else
219  {
220  continue;
221  }
222  }
223 
224  // calculate checksum
225  for (uint16_t i = 2; i < wait_length - 1; i++) // except header, checksum
226  checksum += rxpacket[i];
227  checksum = ~checksum;
228 
229  // verify checksum
230  if (rxpacket[wait_length - 1] == checksum)
231  {
232  result = COMM_SUCCESS;
233  }
234  else
235  {
236  result = COMM_RX_CORRUPT;
237  }
238  break;
239  }
240  else
241  {
242  // remove unnecessary packets
243  for (uint16_t s = 0; s < rx_length - idx; s++)
244  rxpacket[s] = rxpacket[idx + s];
245  //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx);
246  rx_length -= idx;
247  }
248  }
249  else
250  {
251  // check timeout
252  if (port->isPacketTimeout() == true)
253  {
254  if (rx_length == 0)
255  {
256  result = COMM_RX_TIMEOUT;
257  }
258  else
259  {
260  result = COMM_RX_CORRUPT;
261  }
262  break;
263  }
264  }
265  }
266  port->is_using_ = false;
267 
268  return result;
269 }
270 
271 // NOT for BulkRead instruction
272 int Protocol1PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error)
273 {
274  int result = COMM_TX_FAIL;
275 
276  // tx packet
277  result = txPacket(port, txpacket);
278  if (result != COMM_SUCCESS)
279  return result;
280 
281  // (Instruction == BulkRead) == this function is not available.
282  if(txpacket[PKT_INSTRUCTION] == INST_BULK_READ)
283  result = COMM_NOT_AVAILABLE;
284 
285  // (ID == Broadcast ID) == no need to wait for status packet or not available
286  // (Instruction == action) == no need to wait for status packet
287  if (txpacket[PKT_ID] == BROADCAST_ID || txpacket[PKT_INSTRUCTION] == INST_ACTION)
288  {
289  port->is_using_ = false;
290  return result;
291  }
292 
293  // set packet timeout
294  if (txpacket[PKT_INSTRUCTION] == INST_READ)
295  {
296  port->setPacketTimeout((uint16_t)(txpacket[PKT_PARAMETER0+1] + 6));
297  }
298  else
299  {
300  port->setPacketTimeout((uint16_t)6); // HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM
301  }
302 
303  // rx packet
304  do {
305  result = rxPacket(port, rxpacket);
306  } while (result == COMM_SUCCESS && txpacket[PKT_ID] != rxpacket[PKT_ID]);
307 
308  if (result == COMM_SUCCESS && txpacket[PKT_ID] == rxpacket[PKT_ID])
309  {
310  if (error != 0)
311  *error = (uint8_t)rxpacket[PKT_ERROR];
312  }
313 
314  return result;
315 }
316 
317 int Protocol1PacketHandler::ping(PortHandler *port, uint8_t id, uint8_t *error)
318 {
319  return ping(port, id, 0, error);
320 }
321 
322 int Protocol1PacketHandler::ping(PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error)
323 {
324  int result = COMM_TX_FAIL;
325 
326  uint8_t txpacket[6] = {0};
327  uint8_t rxpacket[6] = {0};
328 
329  if (id >= BROADCAST_ID)
330  return COMM_NOT_AVAILABLE;
331 
332  txpacket[PKT_ID] = id;
333  txpacket[PKT_LENGTH] = 2;
334  txpacket[PKT_INSTRUCTION] = INST_PING;
335 
336  result = txRxPacket(port, txpacket, rxpacket, error);
337  if (result == COMM_SUCCESS && model_number != 0)
338  {
339  uint8_t data_read[2] = {0};
340  result = readTxRx(port, id, 0, 2, data_read); // Address 0 : Model Number
341  if (result == COMM_SUCCESS) *model_number = DXL_MAKEWORD(data_read[0], data_read[1]);
342  }
343 
344  return result;
345 }
346 
347 int Protocol1PacketHandler::broadcastPing(PortHandler *port, std::vector<uint8_t> &id_list)
348 {
349  return COMM_NOT_AVAILABLE;
350 }
351 
353 {
354  uint8_t txpacket[6] = {0};
355 
356  txpacket[PKT_ID] = id;
357  txpacket[PKT_LENGTH] = 2;
358  txpacket[PKT_INSTRUCTION] = INST_ACTION;
359 
360  return txRxPacket(port, txpacket, 0);
361 }
362 
363 int Protocol1PacketHandler::reboot(PortHandler *port, uint8_t id, uint8_t *error)
364 {
365  return COMM_NOT_AVAILABLE;
366 }
367 
368 int Protocol1PacketHandler::clearMultiTurn(PortHandler *port, uint8_t id, uint8_t *error)
369 {
370  return COMM_NOT_AVAILABLE;
371 }
372 
373 int Protocol1PacketHandler::factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error)
374 {
375  uint8_t txpacket[6] = {0};
376  uint8_t rxpacket[6] = {0};
377 
378  txpacket[PKT_ID] = id;
379  txpacket[PKT_LENGTH] = 2;
381 
382  return txRxPacket(port, txpacket, rxpacket, error);
383 }
384 
385 int Protocol1PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length)
386 {
387  int result = COMM_TX_FAIL;
388 
389  uint8_t txpacket[8] = {0};
390 
391  if (id >= BROADCAST_ID)
392  return COMM_NOT_AVAILABLE;
393 
394  txpacket[PKT_ID] = id;
395  txpacket[PKT_LENGTH] = 4;
396  txpacket[PKT_INSTRUCTION] = INST_READ;
397  txpacket[PKT_PARAMETER0+0] = (uint8_t)address;
398  txpacket[PKT_PARAMETER0+1] = (uint8_t)length;
399 
400  result = txPacket(port, txpacket);
401 
402  // set packet timeout
403  if (result == COMM_SUCCESS)
404  port->setPacketTimeout((uint16_t)(length+6));
405 
406  return result;
407 }
408 
409 int Protocol1PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error)
410 {
411  int result = COMM_TX_FAIL;
412  uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); //(length+6);
413  //uint8_t *rxpacket = new uint8_t[length+6];
414 
415  if (rxpacket == NULL)
416  return result;
417 
418  do {
419  result = rxPacket(port, rxpacket);
420  } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id);
421 
422  if (result == COMM_SUCCESS && rxpacket[PKT_ID] == id)
423  {
424  if (error != 0)
425  {
426  *error = (uint8_t)rxpacket[PKT_ERROR];
427  }
428  for (uint16_t s = 0; s < length; s++)
429  {
430  data[s] = rxpacket[PKT_PARAMETER0 + s];
431  }
432  //memcpy(data, &rxpacket[PKT_PARAMETER0], length);
433  }
434 
435  free(rxpacket);
436  //delete[] rxpacket;
437  return result;
438 }
439 
440 int Protocol1PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error)
441 {
442  int result = COMM_TX_FAIL;
443 
444  uint8_t txpacket[8] = {0};
445  uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);//(length+6);
446 
447  if (rxpacket == NULL)
448  return result;
449 
450  if (id >= BROADCAST_ID)
451  {
452  free(rxpacket);
453  return COMM_NOT_AVAILABLE;
454  }
455 
456  txpacket[PKT_ID] = id;
457  txpacket[PKT_LENGTH] = 4;
458  txpacket[PKT_INSTRUCTION] = INST_READ;
459  txpacket[PKT_PARAMETER0+0] = (uint8_t)address;
460  txpacket[PKT_PARAMETER0+1] = (uint8_t)length;
461 
462  result = txRxPacket(port, txpacket, rxpacket, error);
463  if (result == COMM_SUCCESS)
464  {
465  if (error != 0)
466  {
467  *error = (uint8_t)rxpacket[PKT_ERROR];
468  }
469  for (uint16_t s = 0; s < length; s++)
470  {
471  data[s] = rxpacket[PKT_PARAMETER0 + s];
472  }
473  //memcpy(data, &rxpacket[PKT_PARAMETER0], length);
474  }
475 
476  free(rxpacket);
477  //delete[] rxpacket;
478  return result;
479 }
480 
481 int Protocol1PacketHandler::read1ByteTx(PortHandler *port, uint8_t id, uint16_t address)
482 {
483  return readTx(port, id, address, 1);
484 }
485 int Protocol1PacketHandler::read1ByteRx(PortHandler *port, uint8_t id, uint8_t *data, uint8_t *error)
486 {
487  uint8_t data_read[1] = {0};
488  int result = readRx(port, id, 1, data_read, error);
489  if (result == COMM_SUCCESS)
490  *data = data_read[0];
491  return result;
492 }
493 int Protocol1PacketHandler::read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error)
494 {
495  uint8_t data_read[1] = {0};
496  int result = readTxRx(port, id, address, 1, data_read, error);
497  if (result == COMM_SUCCESS)
498  *data = data_read[0];
499  return result;
500 }
501 
502 int Protocol1PacketHandler::read2ByteTx(PortHandler *port, uint8_t id, uint16_t address)
503 {
504  return readTx(port, id, address, 2);
505 }
506 int Protocol1PacketHandler::read2ByteRx(PortHandler *port, uint8_t id, uint16_t *data, uint8_t *error)
507 {
508  uint8_t data_read[2] = {0};
509  int result = readRx(port, id, 2, data_read, error);
510  if (result == COMM_SUCCESS)
511  *data = DXL_MAKEWORD(data_read[0], data_read[1]);
512  return result;
513 }
514 int Protocol1PacketHandler::read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error)
515 {
516  uint8_t data_read[2] = {0};
517  int result = readTxRx(port, id, address, 2, data_read, error);
518  if (result == COMM_SUCCESS)
519  *data = DXL_MAKEWORD(data_read[0], data_read[1]);
520  return result;
521 }
522 
523 int Protocol1PacketHandler::read4ByteTx(PortHandler *port, uint8_t id, uint16_t address)
524 {
525  return readTx(port, id, address, 4);
526 }
527 int Protocol1PacketHandler::read4ByteRx(PortHandler *port, uint8_t id, uint32_t *data, uint8_t *error)
528 {
529  uint8_t data_read[4] = {0};
530  int result = readRx(port, id, 4, data_read, error);
531  if (result == COMM_SUCCESS)
532  *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3]));
533  return result;
534 }
535 int Protocol1PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error)
536 {
537  uint8_t data_read[4] = {0};
538  int result = readTxRx(port, id, address, 4, data_read, error);
539  if (result == COMM_SUCCESS)
540  *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3]));
541  return result;
542 }
543 
544 int Protocol1PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
545 {
546  int result = COMM_TX_FAIL;
547 
548  uint8_t *txpacket = (uint8_t *)malloc(length+7);
549  //uint8_t *txpacket = new uint8_t[length+7];
550 
551  if (txpacket == NULL)
552  return result;
553 
554  txpacket[PKT_ID] = id;
555  txpacket[PKT_LENGTH] = length+3;
556  txpacket[PKT_INSTRUCTION] = INST_WRITE;
557  txpacket[PKT_PARAMETER0] = (uint8_t)address;
558 
559  for (uint16_t s = 0; s < length; s++)
560  txpacket[PKT_PARAMETER0+1+s] = data[s];
561  //memcpy(&txpacket[PKT_PARAMETER0+1], data, length);
562 
563  result = txPacket(port, txpacket);
564  port->is_using_ = false;
565 
566  free(txpacket);
567  //delete[] txpacket;
568  return result;
569 }
570 
571 int Protocol1PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error)
572 {
573  int result = COMM_TX_FAIL;
574 
575  uint8_t *txpacket = (uint8_t *)malloc(length+7); //#6->7
576  //uint8_t *txpacket = new uint8_t[length+7];
577  uint8_t rxpacket[6] = {0};
578 
579  if (txpacket == NULL)
580  return result;
581 
582  txpacket[PKT_ID] = id;
583  txpacket[PKT_LENGTH] = length+3;
584  txpacket[PKT_INSTRUCTION] = INST_WRITE;
585  txpacket[PKT_PARAMETER0] = (uint8_t)address;
586 
587  for (uint16_t s = 0; s < length; s++)
588  txpacket[PKT_PARAMETER0+1+s] = data[s];
589  //memcpy(&txpacket[PKT_PARAMETER0+1], data, length);
590 
591  result = txRxPacket(port, txpacket, rxpacket, error);
592 
593  free(txpacket);
594  //delete[] txpacket;
595  return result;
596 }
597 
598 int Protocol1PacketHandler::write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data)
599 {
600  uint8_t data_write[1] = { data };
601  return writeTxOnly(port, id, address, 1, data_write);
602 }
603 int Protocol1PacketHandler::write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error)
604 {
605  uint8_t data_write[1] = { data };
606  return writeTxRx(port, id, address, 1, data_write, error);
607 }
608 
609 int Protocol1PacketHandler::write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data)
610 {
611  uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) };
612  return writeTxOnly(port, id, address, 2, data_write);
613 }
614 int Protocol1PacketHandler::write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error)
615 {
616  uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) };
617  return writeTxRx(port, id, address, 2, data_write, error);
618 }
619 
620 int Protocol1PacketHandler::write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data)
621 {
622  uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) };
623  return writeTxOnly(port, id, address, 4, data_write);
624 }
625 int Protocol1PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error)
626 {
627  uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) };
628  return writeTxRx(port, id, address, 4, data_write, error);
629 }
630 
631 int Protocol1PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
632 {
633  int result = COMM_TX_FAIL;
634 
635  uint8_t *txpacket = (uint8_t *)malloc(length+6);
636  //uint8_t *txpacket = new uint8_t[length+6];
637 
638  if (txpacket == NULL)
639  return result;
640 
641  txpacket[PKT_ID] = id;
642  txpacket[PKT_LENGTH] = length+3;
643  txpacket[PKT_INSTRUCTION] = INST_REG_WRITE;
644  txpacket[PKT_PARAMETER0] = (uint8_t)address;
645 
646  for (uint16_t s = 0; s < length; s++)
647  txpacket[PKT_PARAMETER0+1+s] = data[s];
648  //memcpy(&txpacket[PKT_PARAMETER0+1], data, length);
649 
650  result = txPacket(port, txpacket);
651  port->is_using_ = false;
652 
653  free(txpacket);
654  //delete[] txpacket;
655  return result;
656 }
657 
658 int Protocol1PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error)
659 {
660  int result = COMM_TX_FAIL;
661 
662  uint8_t *txpacket = (uint8_t *)malloc(length+6);
663  //uint8_t *txpacket = new uint8_t[length+6];
664  uint8_t rxpacket[6] = {0};
665 
666  if (txpacket == NULL)
667  return result;
668 
669  txpacket[PKT_ID] = id;
670  txpacket[PKT_LENGTH] = length+3;
671  txpacket[PKT_INSTRUCTION] = INST_REG_WRITE;
672  txpacket[PKT_PARAMETER0] = (uint8_t)address;
673 
674  for (uint16_t s = 0; s < length; s++)
675  txpacket[PKT_PARAMETER0+1+s] = data[s];
676  //memcpy(&txpacket[PKT_PARAMETER0+1], data, length);
677 
678  result = txRxPacket(port, txpacket, rxpacket, error);
679 
680  free(txpacket);
681  //delete[] txpacket;
682  return result;
683 }
684 
685 int Protocol1PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length)
686 {
687  return COMM_NOT_AVAILABLE;
688 }
689 
690 int Protocol1PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length)
691 {
692  int result = COMM_TX_FAIL;
693 
694  uint8_t *txpacket = (uint8_t *)malloc(param_length+8);
695  // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM
696  //uint8_t *txpacket = new uint8_t[param_length + 8];
697 
698  if (txpacket == NULL)
699  return result;
700 
701  txpacket[PKT_ID] = BROADCAST_ID;
702  txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM
703  txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE;
704  txpacket[PKT_PARAMETER0+0] = start_address;
705  txpacket[PKT_PARAMETER0+1] = data_length;
706 
707  for (uint16_t s = 0; s < param_length; s++)
708  txpacket[PKT_PARAMETER0+2+s] = param[s];
709  //memcpy(&txpacket[PKT_PARAMETER0+2], param, param_length);
710 
711  result = txRxPacket(port, txpacket, 0, 0);
712 
713  free(txpacket);
714  //delete[] txpacket;
715  return result;
716 }
717 
718 int Protocol1PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length)
719 {
720  int result = COMM_TX_FAIL;
721 
722  uint8_t *txpacket = (uint8_t *)malloc(param_length+7);
723  // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM
724  //uint8_t *txpacket = new uint8_t[param_length + 7];
725 
726  if (txpacket == NULL)
727  return result;
728 
729  txpacket[PKT_ID] = BROADCAST_ID;
730  txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM
731  txpacket[PKT_INSTRUCTION] = INST_BULK_READ;
732  txpacket[PKT_PARAMETER0+0] = 0x00;
733 
734  for (uint16_t s = 0; s < param_length; s++)
735  txpacket[PKT_PARAMETER0+1+s] = param[s];
736  //memcpy(&txpacket[PKT_PARAMETER0+1], param, param_length);
737 
738  result = txPacket(port, txpacket);
739  if (result == COMM_SUCCESS)
740  {
741  int wait_length = 0;
742  for (uint16_t i = 0; i < param_length; i += 3)
743  wait_length += param[i] + 7;
744  port->setPacketTimeout((uint16_t)wait_length);
745  }
746 
747  free(txpacket);
748  //delete[] txpacket;
749  return result;
750 }
751 
752 int Protocol1PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length)
753 {
754  return COMM_NOT_AVAILABLE;
755 }
#define COMM_RX_FAIL
#define DXL_MAKEDWORD(a, b)
#define TXPACKET_MAX_LEN
int regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error=0)
The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynami...
int clearMultiTurn(PortHandler *port, uint8_t id, uint8_t *error=0)
(Available only in Protocol 2.0) The function that reset multi-turn revolution information of Dynamix...
static Protocol1PacketHandler * unique_instance_
virtual void clearPort()=0
The function that clears the port The function clears the port.
int syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length)
The function that transmits Sync Write instruction packet The function makes an instruction packet w...
#define PKT_ID
#define INST_READ
int read1ByteRx(PortHandler *port, uint8_t id, uint8_t *data, uint8_t *error=0)
The function that calls Protocol1PacketHandler::readRx() function and reads 1 byte data on the packet...
int read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error=0)
The function that calls Protocol1PacketHandler::readTxRx() function for reading 1 byte data The func...
#define ERRBIT_RANGE
int writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
The function that transmits INST_WRITE instruction packet with the data for write The function makes...
#define COMM_SUCCESS
int regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynami...
int read1ByteTx(PortHandler *port, uint8_t id, uint16_t address)
The function that calls Protocol1PacketHandler::readTx() function for reading 1 byte data The functi...
The class for control Dynamixel by using Protocol1.0.
#define DXL_MAKEWORD(a, b)
#define RXPACKET_MAX_LEN
XmlRpcServer s
#define ERRBIT_OVERLOAD
int writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error=0)
The function that transmits INST_WRITE instruction packet with the data for write, and receives the packet The function makes an instruction packet with INST_WRITE and the data for write, transmits and receives the packet with Protocol1PacketHandler::txRxPacket(), gets the error from the packet.
#define ERRBIT_ANGLE
int read2ByteTx(PortHandler *port, uint8_t id, uint16_t address)
The function that calls Protocol1PacketHandler::readTx() function for reading 2 byte data The functi...
#define PKT_INSTRUCTION
int write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error=0)
The function that calls Protocol1PacketHandler::writeTxRx() for writing 1 byte data and receives the ...
#define COMM_RX_TIMEOUT
#define COMM_TX_FAIL
#define PKT_PARAMETER0
#define ERRBIT_INSTRUCTION
const char * getTxRxResult(int result)
The function that gets description of communication result.
int read2ByteRx(PortHandler *port, uint8_t id, uint16_t *data, uint8_t *error=0)
The function that calls Protocol1PacketHandler::readRx() function and reads 2 byte data on the packet...
#define PKT_ERROR
int reboot(PortHandler *port, uint8_t id, uint8_t *error=0)
(Available only in Protocol 2.0) The function that makes Dynamixel reboot
#define ERRBIT_VOLTAGE
int read4ByteTx(PortHandler *port, uint8_t id, uint16_t address)
The function that calls Protocol1PacketHandler::readTx() function for reading 4 byte data The functi...
int bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length)
(Available only on Dynamixel MX / X series) The function that transmits Bulk Read instruction packet ...
#define ERRBIT_CHECKSUM
int write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error=0)
The function that calls Protocol1PacketHandler::writeTxRx() for writing 2 byte data and receives the ...
#define COMM_RX_WAITING
int factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error=0)
The function that makes Dynamixel reset as it was produced in the factory The function makes an inst...
#define ERRBIT_OVERHEAT
int txPacket(PortHandler *port, uint8_t *txpacket)
The function that transmits the instruction packet txpacket via PortHandler port. The function clear...
#define DXL_LOBYTE(w)
#define INST_FACTORY_RESET
#define INST_WRITE
int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data)
The function that calls Protocol1PacketHandler::writeTxOnly() for writing 2 byte data The function c...
#define INST_BULK_READ
int readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error=0)
The function that receives the packet and reads the data in the packet The function receives the pac...
The class for port control that inherits PortHandlerLinux, PortHandlerWindows, PortHandlerMac, or PortHandlerArduino.
Definition: port_handler.h:56
#define INST_REG_WRITE
#define COMM_TX_ERROR
#define INST_SYNC_WRITE
#define PKT_HEADER1
int write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error=0)
The function that calls Protocol1PacketHandler::writeTxRx() for writing 4 byte data and receives the ...
virtual int writePort(uint8_t *packet, int length)=0
The function that writes bytes on the port buffer The function writes bytes on the port buffer...
const char * getRxPacketError(uint8_t error)
The function that gets description of hardware error.
int broadcastPing(PortHandler *port, std::vector< uint8_t > &id_list)
(Available only in Protocol 2.0) The function that pings all connected Dynamixel
#define DXL_HIWORD(l)
int read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error=0)
The function that calls Protocol1PacketHandler::readTxRx() function for reading 4 byte data The func...
#define INST_PING
virtual bool isPacketTimeout()=0
The function that checks whether packet timeout is occurred The function checks whether current time...
#define DXL_HIBYTE(w)
int read4ByteRx(PortHandler *port, uint8_t id, uint32_t *data, uint8_t *error=0)
The function that calls Protocol1PacketHandler::readRx() function and reads 4 byte data on the packet...
int rxPacket(PortHandler *port, uint8_t *rxpacket)
The function that receives packet (rxpacket) during designated time via PortHandler port The functio...
int ping(PortHandler *port, uint8_t id, uint8_t *error=0)
The function that pings Dynamixel but doesn&#39;t take its model number The function calls Protocol1Pack...
int read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error=0)
The function that calls Protocol1PacketHandler::readTxRx() function for reading 2 byte data The func...
int bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length)
(Available only in Protocol 2.0) The function that transmits Bulk Write instruction packet ...
#define COMM_RX_CORRUPT
virtual int readPort(uint8_t *packet, int length)=0
The function that reads bytes from the port buffer The function gets bytes from the port buffer...
#define PKT_HEADER0
int syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length)
(Available only in Protocol 2.0) The function that transmits Sync Read instruction packet ...
#define COMM_NOT_AVAILABLE
#define COMM_PORT_BUSY
int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data)
The function that calls Protocol1PacketHandler::writeTxOnly() for writing 4 byte data The function c...
bool is_using_
shows whether the port is in use
Definition: port_handler.h:67
int txRxPacket(PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error=0)
The function that transmits packet (txpacket) and receives packet (rxpacket) during designated time v...
int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data)
The function that calls Protocol1PacketHandler::writeTxOnly() for writing 1 byte data The function c...
#define INST_ACTION
virtual void setPacketTimeout(uint16_t packet_length)=0
The function that sets and starts stopwatch for watching packet timeout The function sets the stopwa...
#define DXL_LOWORD(l)
#define BROADCAST_ID
int readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error=0)
The function that transmits INST_READ instruction packet, and read data from received packet The fun...
int action(PortHandler *port, uint8_t id)
The function that makes Dynamixels run as written in the Dynamixel register The function makes an in...
int readTx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length)
The function that transmits INST_READ instruction packet The function makes an instruction packet wi...
#define PKT_LENGTH


dynamixel_sdk
Author(s): Gilbert , Zerom , Darby Lim , Leon
autogenerated on Fri Apr 16 2021 02:25:55