dynamixel_workbench.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2018 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 /* Authors: Taehun Lim (Darby), Ryan Shim */
18 
19 #include "../../include/dynamixel_workbench_toolbox/dynamixel_workbench.h"
20 
21 static const uint8_t WHEEL_MODE = 1;
22 static const uint8_t JOINT_MODE = 2;
23 
24 static const uint8_t CURRENT_CONTROL_MODE = 0;
25 static const uint8_t VELOCITY_CONTROL_MODE = 1;
26 static const uint8_t POSITION_CONTROL_MODE = 3;
27 static const uint8_t EXTENDED_POSITION_CONTROL_MODE = 4;
28 static const uint8_t CURRENT_BASED_POSITION_CONTROL_MODE = 5;
29 static const uint8_t PWM_CONTROL_MODE = 16;
30 static const uint8_t TORQUE_CONTROL_MODE = 100;
31 static const uint8_t MULTI_TURN_MODE = 101;
32 
33 static const char* model_name = NULL;
34 static const ModelInfo* model_info = NULL;
35 
36 DynamixelWorkbench::DynamixelWorkbench(){}
37 
38 DynamixelWorkbench::~DynamixelWorkbench(){}
39 
40 bool DynamixelWorkbench::torque(uint8_t id, int32_t onoff, const char **log)
41 {
42  bool result = false;
43 
44  result = itemWrite(id, "Torque_Enable", (int32_t)onoff, log);
45  if (result == false)
46  {
47  if (log != NULL) *log = "[DynamixelWorkbench] Failed to change torque status!";
48  return false;
49  }
50 
51  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to change torque status!";
52  return result;
53 }
54 
55 bool DynamixelWorkbench::torqueOn(uint8_t id, const char **log)
56 {
57  bool result = false;
58 
59  result = torque(id, 1, log);
60 
61  return result;
62 }
63 
64 bool DynamixelWorkbench::torqueOff(uint8_t id, const char **log)
65 {
66  bool result = false;
67 
68  result = torque(id, 0, log);
69 
70  return result;
71 }
72 
73 bool DynamixelWorkbench::changeID(uint8_t id, uint8_t new_id, const char **log)
74 {
75  bool result = false;
76 
77  result = torqueOff(id, log);
78  if (result == false) return false;
79 
80  result = writeRegister(id, "ID", new_id, log);
81  if (result == false)
82  {
83  if (log != NULL) *log = "[DynamixelWorkbench] Failed to change ID!";
84  return false;
85  }
86  // millis(1000);
87 
88  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to change ID!";
89  return result;
90 }
91 
92 bool DynamixelWorkbench::changeBaudrate(uint8_t id, uint32_t new_baudrate, const char **log)
93 {
94  bool result = false;
95 
96  result = torqueOff(id, log);
97  if (result == false) return false;
98 
99  if (getProtocolVersion() == 1.0f)
100  {
101  switch (new_baudrate)
102  {
103  case 9600:
104  result = writeRegister(id, "Baud_Rate", 207, log);
105  break;
106 
107  case 19200:
108  result = writeRegister(id, "Baud_Rate", 103, log);
109  break;
110 
111  case 57600:
112  result = writeRegister(id, "Baud_Rate", 34, log);
113  break;
114 
115  case 115200:
116  result = writeRegister(id, "Baud_Rate", 16, log);
117  break;
118 
119  case 200000:
120  result = writeRegister(id, "Baud_Rate", 9, log);
121  break;
122 
123  case 250000:
124  result = writeRegister(id, "Baud_Rate", 7, log);
125  break;
126 
127  case 400000:
128  result = writeRegister(id, "Baud_Rate", 4, log);
129  break;
130 
131  case 500000:
132  result = writeRegister(id, "Baud_Rate", 3, log);
133  break;
134 
135  case 1000000:
136  result = writeRegister(id, "Baud_Rate", 1, log);
137  break;
138 
139  case 2250000:
140  result = writeRegister(id, "Baud_Rate", 250, log);
141  break;
142 
143  case 2500000:
144  result = writeRegister(id, "Baud_Rate", 251, log);
145  break;
146 
147  case 3000000:
148  result = writeRegister(id, "Baud_Rate", 252, log);
149  break;
150 
151  default:
152  result = writeRegister(id, "Baud_Rate", 34, log);
153  break;
154  }
155  }
156  else if (getProtocolVersion() == 2.0f)
157  {
158  switch (new_baudrate)
159  {
160  case 9600:
161  result = writeRegister(id, "Baud_Rate", 0, log);
162  break;
163 
164  case 57600:
165  result = writeRegister(id, "Baud_Rate", 1, log);
166  break;
167 
168  case 115200:
169  result = writeRegister(id, "Baud_Rate", 2, log);
170  break;
171 
172  case 1000000:
173  result = writeRegister(id, "Baud_Rate", 3, log);
174  break;
175 
176  case 2000000:
177  result = writeRegister(id, "Baud_Rate", 4, log);
178  break;
179 
180  case 3000000:
181  result = writeRegister(id, "Baud_Rate", 5, log);
182  break;
183 
184  case 4000000:
185  result = writeRegister(id, "Baud_Rate", 6, log);
186  break;
187 
188  case 4500000:
189  result = writeRegister(id, "Baud_Rate", 7, log);
190  break;
191 
192  case 10500000:
193  result = writeRegister(id, "Baud_Rate", 8, log);
194  break;
195 
196  default:
197  result = writeRegister(id, "Baud_Rate", 1, log);
198  break;
199  }
200  }
201 #if defined(__OPENCR__) || defined(__OPENCM904__)
202  delay(2000);
203 #else
204  usleep(1000*2000);
205 #endif
206 
207  if (result == false)
208  {
209  if (log != NULL) *log = "[DynamixelWorkbench] Failed to change Baud Rate!";
210  return result;
211  }
212 
213  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to change Baud Rate!";
214  return result;
215 }
216 
217 bool DynamixelWorkbench::changeProtocolVersion(uint8_t id, uint8_t version, const char **log)
218 {
219  bool result = false;
220 
221  model_name = getModelName(id, log);
222  if (model_name == NULL) return false;
223 
224  if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) ||
225  !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) ||
226  !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) ||
227  !strncmp(model_name, "XM", strlen("XM")) ||
228  !strncmp(model_name, "XL330", strlen("XL330")) ||
229  !strncmp(model_name, "XL430", strlen("XL430")) ||
230  !strncmp(model_name, "XC430", strlen("XC430")) ||
231  !strncmp(model_name, "XH", strlen("XH")) ||
232  !strncmp(model_name, "XW", strlen("XW")))
233  {
234  result = writeRegister(id, "Protocol_Version", version, log);
235  if (result == false)
236  {
237  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set protocol version!";
238  return false;
239  }
240  }
241 
242  result = setPacketHandler((float)version, log);
243 
244  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set protocol version!";
245  return result;
246 }
247 
248 bool DynamixelWorkbench::itemWrite(uint8_t id, const char *item_name, int32_t data, const char **log)
249 {
250  return writeRegister(id, item_name, data, log);
251 }
252 
253 bool DynamixelWorkbench::itemRead(uint8_t id, const char *item_name, int32_t *data, const char **log)
254 {
255  return readRegister(id, item_name, data, log);
256 }
257 
258 bool DynamixelWorkbench::led(uint8_t id, int32_t onoff, const char **log)
259 {
260  bool result = false;
261 
262  result = writeRegister(id, "LED", onoff, log);
263  if (result == false)
264  {
265  if (log != NULL) *log = "[DynamixelWorkbench] Failed to change led status!";
266  return false;
267  }
268 
269  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to change led status!";
270  return result;
271 }
272 
273 bool DynamixelWorkbench::ledOn(uint8_t id, const char **log)
274 {
275  bool result = false;
276 
277  result = led(id, 1, log);
278 
279  return result;
280 }
281 
282 bool DynamixelWorkbench::ledOff(uint8_t id, const char **log)
283 {
284  bool result = false;
285 
286  result = led(id, 0, log);
287 
288  return result;
289 }
290 
291 bool DynamixelWorkbench::setNormalDirection(uint8_t id, const char **log)
292 {
293  bool result = false;
294  int32_t data = 0;
295 
296  model_name = getModelName(id, log);
297  if (model_name == NULL) return false;
298 
299  if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) ||
300  !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) ||
301  !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) ||
302  !strncmp(model_name, "XM", strlen("XM")) ||
303  !strncmp(model_name, "XL330", strlen("XL330")) ||
304  !strncmp(model_name, "XL430", strlen("XL430")) ||
305  !strncmp(model_name, "XC430", strlen("XC430")) ||
306  !strncmp(model_name, "XH", strlen("XH")) ||
307  !strncmp(model_name, "XW", strlen("XW")))
308  {
309  result = readRegister(id, "Drive_Mode", &data, log);
310 
311  data = data & 0b00000100;
312  result = writeRegister(id, "Drive_Mode", data, log);
313  if (result == false)
314  {
315  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set normal direction!";
316  return false;
317  }
318  }
319 
320  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set normal direction!";
321  return result;
322 }
323 
324 bool DynamixelWorkbench::setReverseDirection(uint8_t id, const char **log)
325 {
326  bool result = false;
327  int32_t data = 0;
328 
329  model_name = getModelName(id, log);
330  if (model_name == NULL) return false;
331 
332  if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) ||
333  !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) ||
334  !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) ||
335  !strncmp(model_name, "XM", strlen("XM")) ||
336  !strncmp(model_name, "XL330", strlen("XL330")) ||
337  !strncmp(model_name, "XL430", strlen("XL430")) ||
338  !strncmp(model_name, "XC430", strlen("XC430")) ||
339  !strncmp(model_name, "XH", strlen("XH")) ||
340  !strncmp(model_name, "XW", strlen("XW")))
341  {
342  result = readRegister(id, "Drive_Mode", &data, log);
343 
344  data = data | 0b00000001;
345  result = writeRegister(id, "Drive_Mode", data, log);
346  if (result == false)
347  {
348  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set reverse direction!";
349  return false;
350  }
351  }
352 
353  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set reverse direction!";
354  return result;
355 }
356 
357 bool DynamixelWorkbench::setVelocityBasedProfile(uint8_t id, const char **log)
358 {
359  bool result = false;
360  int32_t data = 0;
361 
362  model_name = getModelName(id, log);
363  if (model_name == NULL) return false;
364 
365  if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) ||
366  !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) ||
367  !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) ||
368  !strncmp(model_name, "XM", strlen("XM")) ||
369  !strncmp(model_name, "XL330", strlen("XL330")) ||
370  !strncmp(model_name, "XL430", strlen("XL430")) ||
371  !strncmp(model_name, "XC430", strlen("XC430")) ||
372  !strncmp(model_name, "XH", strlen("XH")) ||
373  !strncmp(model_name, "XW", strlen("XW")))
374  {
375  result = readRegister(id, "Drive_Mode", &data, log);
376 
377  data = data & 0b00000001;
378  result = writeRegister(id, "Drive_Mode", data, log);
379  if (result == false)
380  {
381  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set velocity based profile!";
382  return false;
383  }
384  }
385 
386  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set velocity based profile!";
387  return result;
388 }
389 
390 bool DynamixelWorkbench::setTimeBasedProfile(uint8_t id, const char **log)
391 {
392  bool result = false;
393  int32_t data = 0;
394 
395  model_name = getModelName(id, log);
396  if (model_name == NULL) return false;
397 
398  if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) ||
399  !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) ||
400  !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) ||
401  !strncmp(model_name, "XM", strlen("XM")) ||
402  !strncmp(model_name, "XL330", strlen("XL330")) ||
403  !strncmp(model_name, "XL430", strlen("XL430")) ||
404  !strncmp(model_name, "XC430", strlen("XC430")) ||
405  !strncmp(model_name, "XH", strlen("XH")) ||
406  !strncmp(model_name, "XW", strlen("XW")))
407  {
408  result = readRegister(id, "Drive_Mode", &data, log);
409 
410  data = data | 0b00000100;
411  result = writeRegister(id, "Drive_Mode", data, log);
412  if (result == false)
413  {
414  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set time based profile!";
415  return false;
416  }
417  }
418 
419  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set time based profile!";
420  return result;
421 }
422 
423 bool DynamixelWorkbench::setSecondaryID(uint8_t id, uint8_t secondary_id, const char **log)
424 {
425  bool result = false;
426 
427  model_name = getModelName(id, log);
428  if (model_name == NULL) return false;
429 
430  if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) ||
431  !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) ||
432  !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) ||
433  !strncmp(model_name, "XM", strlen("XM")) ||
434  !strncmp(model_name, "XL330", strlen("XL330")) ||
435  !strncmp(model_name, "XL430", strlen("XL430")) ||
436  !strncmp(model_name, "XC430", strlen("XC430")) ||
437  !strncmp(model_name, "XH", strlen("XH")) ||
438  !strncmp(model_name, "XW", strlen("XW")) ||
439  !strncmp(model_name, "RH", strlen("RH")))
440  {
441  result = torqueOff(id, log);
442  if (result == false) return false;
443 
444  result = writeRegister(id, "Secondary_ID", secondary_id, log);
445  if (result == false)
446  {
447  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set secondary ID!";
448  return false;
449  }
450  }
451 
452  // millis(1000);
453 
454  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set secondary ID!";
455  return result;
456 }
457 
458 bool DynamixelWorkbench::setPositionControlMode(uint8_t id, const char **log)
459 {
460  bool result = false;
461 
462  result = setOperatingMode(id, POSITION_CONTROL_MODE, log);
463 
464  if (result == false)
465  {
466  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Position Control Mode!";
467  return false;
468  }
469 
470  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Position Control Mode!";
471  return result;
472 }
473 
474 bool DynamixelWorkbench::setVelocityControlMode(uint8_t id, const char **log)
475 {
476  bool result = false;
477 
478  result = setOperatingMode(id, VELOCITY_CONTROL_MODE, log);
479 
480  if (result == false)
481  {
482  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Velocity Control Mode!";
483  return false;
484  }
485 
486  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Velocity Control Mode!";
487  return result;
488 }
489 
490 bool DynamixelWorkbench::setCurrentControlMode(uint8_t id, const char **log)
491 {
492  bool result = false;
493 
494  result = setOperatingMode(id, CURRENT_CONTROL_MODE, log);
495 
496  if (result == false)
497  {
498  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Current Control Mode!";
499  return false;
500  }
501 
502  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Current Control Mode!";
503  return result;
504 }
505 
506 bool DynamixelWorkbench::setTorqueControlMode(uint8_t id, const char **log)
507 {
508  bool result = false;
509 
510  result = setOperatingMode(id, TORQUE_CONTROL_MODE, log);
511 
512  if (result == false)
513  {
514  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Torque Control Mode!";
515  return false;
516  }
517 
518  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Torque Control Mode!";
519  return result;
520 }
521 
522 bool DynamixelWorkbench::setExtendedPositionControlMode(uint8_t id, const char **log)
523 {
524  bool result = false;
525 
526  result = setOperatingMode(id, EXTENDED_POSITION_CONTROL_MODE, log);
527 
528  if (result == false)
529  {
530  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Extended Position Control Mode!";
531  return false;
532  }
533 
534  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Extended Position Control Mode!";
535  return result;
536 }
537 
538 bool DynamixelWorkbench::setMultiTurnControlMode(uint8_t id, const char **log)
539 {
540  bool result = false;
541 
542  result = setOperatingMode(id, MULTI_TURN_MODE, log);
543 
544  if (result == false)
545  {
546  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Multi-Turn Control Mode!";
547  return false;
548  }
549 
550  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Multi-Turn Control Mode!";
551  return result;
552 }
553 
554 bool DynamixelWorkbench::setCurrentBasedPositionControlMode(uint8_t id, const char **log)
555 {
556  bool result = false;
557 
558  result = setOperatingMode(id, CURRENT_BASED_POSITION_CONTROL_MODE, log);
559 
560  if (result == false)
561  {
562  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Current Based Position Control Mode!";
563  return false;
564  }
565 
566  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Current Based Position Control Mode!";
567  return result;
568 }
569 
570 bool DynamixelWorkbench::setPWMControlMode(uint8_t id, const char **log)
571 {
572  bool result = false;
573 
574  result = setOperatingMode(id, PWM_CONTROL_MODE, log);
575 
576  if (result == false)
577  {
578  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set PWM Control Mode!";
579  return false;
580  }
581 
582  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set PWM Control Mode!";
583  return result;
584 }
585 
586 bool DynamixelWorkbench::setOperatingMode(uint8_t id, uint8_t index, const char **log)
587 {
588  bool result = false;
589 
590  model_name = getModelName(id, log);
591  if (model_name == NULL) return false;
592 
593  if (getProtocolVersion() == 1.0)
594  {
595  if (index == POSITION_CONTROL_MODE)
596  {
597  if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) ||
598  !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) ||
599  !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) ||
600  !strncmp(model_name, "XL430", strlen("XL430")) ||
601  !strncmp(model_name, "XC430", strlen("XC430")) ||
602  !strncmp(model_name, "XM", strlen("XM")) ||
603  !strncmp(model_name, "XH", strlen("XH")) ||
604  !strncmp(model_name, "XW", strlen("XW")) ||
605  !strncmp(model_name, "PRO", strlen("PRO")) ||
606  !strncmp(model_name, "RH", strlen("RH")))
607  {
608  result = writeRegister(id, "Operating_Mode", POSITION_CONTROL_MODE, log);
609  }
610  else if (!strncmp(model_name, "AX", 2) || !strncmp(model_name, "RX", 2))
611  {
612  result = writeRegister(id, "CW_Angle_Limit", 0, log);
613  result = writeRegister(id, "CCW_Angle_Limit", 1023, log);
614  }
615  else
616  {
617  result = writeRegister(id, "CW_Angle_Limit", 0, log);
618  result = writeRegister(id, "CCW_Angle_Limit", 4095, log);
619  }
620  }
621  else if (index == VELOCITY_CONTROL_MODE)
622  {
623  if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) ||
624  !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) ||
625  !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) ||
626  !strncmp(model_name, "XL430", strlen("XL430")) ||
627  !strncmp(model_name, "XC430", strlen("XC430")) ||
628  !strncmp(model_name, "XM", strlen("XM")) ||
629  !strncmp(model_name, "XH", strlen("XH")) ||
630  !strncmp(model_name, "XW", strlen("XW")) ||
631  !strncmp(model_name, "PRO", strlen("PRO")))
632  {
633  result = writeRegister(id, "Operating_Mode", VELOCITY_CONTROL_MODE);
634  }
635  else
636  {
637  result = writeRegister(id, "CW_Angle_Limit", 0, log);
638  result = writeRegister(id, "CCW_Angle_Limit", 0, log);
639  }
640  }
641  else if (index == CURRENT_CONTROL_MODE)
642  {
643  if (!strncmp(model_name, "XM", strlen("XM")) ||
644  !strncmp(model_name, "XH", strlen("XH")) ||
645  !strncmp(model_name, "XW", strlen("XW")) ||
646  !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) ||
647  !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) ||
648  !strncmp(model_name, "RH", strlen("RH")))
649  {
650  result = writeRegister(id, "Operating_Mode", CURRENT_CONTROL_MODE, log);
651  }
652  }
653  else if (index == TORQUE_CONTROL_MODE)
654  {
655  if (!strncmp(model_name, "MX-64", strlen("MX-64")) ||
656  !strncmp(model_name, "MX-106", strlen("MX-106")) )
657  {
658  result = writeRegister(id, "Torque_Control_Mode_Enable", 1, log);
659  }
660  }
661  else if (index == MULTI_TURN_MODE)
662  {
663  if (!strncmp(model_name, "MX-64", strlen("MX-64")) ||
664  !strncmp(model_name, "MX-106", strlen("MX-106")) )
665  {
666  result = writeRegister(id, "CW_Angle_Limit", 4095, log);
667  result = writeRegister(id, "CCW_Angle_Limit", 4095, log);
668  }
669  }
670  else if (index == CURRENT_BASED_POSITION_CONTROL_MODE)
671  {
672  if (!strncmp(model_name, "MX-64-2", strlen("MX-64-2")) ||
673  !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) ||
674  !strncmp(model_name, "XM", strlen("XM")) ||
675  !strncmp(model_name, "XH", strlen("XH")) ||
676  !strncmp(model_name, "XW", strlen("XW")) ||
677  !strncmp(model_name, "RH", strlen("RH")))
678  {
679  result = writeRegister(id, "Operating_Mode", CURRENT_BASED_POSITION_CONTROL_MODE, log);
680  }
681  }
682  else if (index == PWM_CONTROL_MODE)
683  {
684  if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) ||
685  !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) ||
686  !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) ||
687  !strncmp(model_name, "XM", strlen("XM")) ||
688  !strncmp(model_name, "XH", strlen("XH")) ||
689  !strncmp(model_name, "XW", strlen("XW")))
690  {
691  result = writeRegister(id, "Operating_Mode", PWM_CONTROL_MODE, log);
692  }
693  }
694  }
695  else if (getProtocolVersion() == 2.0)
696  {
697  if (index == POSITION_CONTROL_MODE)
698  {
699  if (!strncmp(model_name, "XL-320", strlen("XL-320")))
700  result = writeRegister(id, "Control_Mode", JOINT_MODE, log);
701  else
702  result = writeRegister(id, "Operating_Mode", POSITION_CONTROL_MODE, log);
703  }
704  else if (index == VELOCITY_CONTROL_MODE)
705  {
706  if (!strncmp(model_name, "XL-320", strlen("XL-320")))
707  result = writeRegister(id, "Control_Mode", WHEEL_MODE, log);
708  else
709  result = writeRegister(id, "Operating_Mode", VELOCITY_CONTROL_MODE, log);
710  }
711  else if (index == CURRENT_CONTROL_MODE)
712  {
713  if (!strncmp(model_name, "XL330", strlen("XL330")) ||
714  !strncmp(model_name, "XM", strlen("XM")) ||
715  !strncmp(model_name, "XH", strlen("XH")) ||
716  !strncmp(model_name, "XW", strlen("XW")) ||
717  !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) ||
718  !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) ||
719  !strncmp(model_name, "RH", strlen("RH")) ||
720  !strncmp(model_name, "PRO-PLUS", strlen("PRO-PLUS")))
721  {
722  result = writeRegister(id, "Operating_Mode", CURRENT_CONTROL_MODE, log);
723  }
724  }
725  else if (index == TORQUE_CONTROL_MODE)
726  {
727  if (!strncmp(model_name, "PRO", strlen("PRO")) ||
728  strncmp(model_name, "PRO-L42", strlen("PRO-L42")) )
729  {
730  result = writeRegister(id, "Operating_Mode", 0, log);
731  }
732  }
733  else if (index == EXTENDED_POSITION_CONTROL_MODE)
734  {
735  if (!strncmp(model_name, "PRO", strlen("PRO")) ||
736  strncmp(model_name, "PRO-L42", strlen("PRO-L42")) )
737  {
738  result = writeRegister(id, "Operating_Mode", EXTENDED_POSITION_CONTROL_MODE, log);
739  }
740  }
741  else if (index == CURRENT_BASED_POSITION_CONTROL_MODE)
742  {
743  if (!strncmp(model_name, "MX-64-2", strlen("MX-64-2")) ||
744  !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) ||
745  !strncmp(model_name, "XL330", strlen("XL330")) ||
746  !strncmp(model_name, "XM", strlen("XM")) ||
747  !strncmp(model_name, "XH", strlen("XH")) ||
748  !strncmp(model_name, "XW", strlen("XW")) ||
749  !strncmp(model_name, "RH", strlen("RH")))
750  {
751  result = writeRegister(id, "Operating_Mode", CURRENT_BASED_POSITION_CONTROL_MODE, log);
752  }
753  }
754  else if (index == PWM_CONTROL_MODE)
755  {
756  if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) ||
757  !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) ||
758  !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) ||
759  !strncmp(model_name, "XL330", strlen("XL330")) ||
760  !strncmp(model_name, "XM", strlen("XM")) ||
761  !strncmp(model_name, "XH", strlen("XH")) ||
762  !strncmp(model_name, "XW", strlen("XW")) ||
763  !strncmp(model_name, "XL430", strlen("XL430")) ||
764  !strncmp(model_name, "XC430", strlen("XC430")) ||
765  !strncmp(model_name, "PRO", strlen("PRO")))
766  {
767  result = writeRegister(id, "Operating_Mode", PWM_CONTROL_MODE, log);
768  }
769  }
770  }
771 
772  if (result == false)
773  {
774  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Operating Mode!";
775  return false;
776  }
777 
778  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Operating Mode!";
779  return result;
780 }
781 
782 
783 bool DynamixelWorkbench::jointMode(uint8_t id, int32_t velocity, int32_t acceleration, const char **log)
784 {
785  bool result = false;
786 
787  model_name = getModelName(id, log);
788  if (model_name == NULL) return false;
789 
790  result = torqueOff(id, log);
791  if (result == false) return false;
792 
793  result = setPositionControlMode(id, log);
794  if (result == false) return false;
795 
796  if (getProtocolVersion() == 1.0)
797  {
798  if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) ||
799  !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) ||
800  !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) ||
801  !strncmp(model_name, "XL430", strlen("XL430")) ||
802  !strncmp(model_name, "XC430", strlen("XC430")) ||
803  !strncmp(model_name, "XM", strlen("XM")) ||
804  !strncmp(model_name, "XH", strlen("XH")) ||
805  !strncmp(model_name, "XW", strlen("XW")))
806  {
807  result = writeRegister(id, "Profile_Acceleration", acceleration, log);
808  result = writeRegister(id, "Profile_Velocity", velocity, log);
809  }
810  else if (!strncmp(model_name, "MX-28", strlen("MX-28")) ||
811  !strncmp(model_name, "MX-64", strlen("MX-64")) ||
812  !strncmp(model_name, "MX-106", strlen("MX-106")))
813  {
814  result = writeRegister(id, "Moving_Speed", velocity, log);
815  result = writeRegister(id, "Goal_Acceleration", acceleration, log);
816  }
817  else
818  {
819  result = writeRegister(id, "Moving_Speed", velocity, log);
820  }
821  }
822  else if (getProtocolVersion() == 2.0)
823  {
824  if (!strncmp(model_name, "XL-320", strlen("XL-320")))
825  {
826  result = writeRegister(id, "Moving_Speed", velocity, log);
827  }
828  else if (!strncmp(model_name, "PRO-M42-10-S260-R-A", strlen("PRO-M42-10-S260-R-A")) ||
829  !strncmp(model_name, "PRO-M54-40-S250-R-A", strlen("PRO-M54-40-S250-R-A")) ||
830  !strncmp(model_name, "PRO-M54-60-S250-R-A", strlen("PRO-M54-60-S250-R-A")) ||
831  !strncmp(model_name, "PRO-H42-20-S300-R-A", strlen("PRO-H42-20-S300-R-A")) ||
832  !strncmp(model_name, "PRO-H54-100-S500-R-A", strlen("PRO-H54-100-S500-R-A")) ||
833  !strncmp(model_name, "PRO-H54-200-S500-R-A", strlen("PRO-H54-200-S500-R-A")) ||
834  !strncmp(model_name, "PRO-PLUS", strlen("PRO-PLUS")))
835  {
836  result = writeRegister(id, "Profile_Acceleration", acceleration, log);
837  result = writeRegister(id, "Profile_Velocity", velocity, log);
838  }
839  else if (!strncmp(model_name, "PRO-L", strlen("PRO-L")) ||
840  !strncmp(model_name, "PRO-M", strlen("PRO-M")) ||
841  !strncmp(model_name, "PRO-H", strlen("PRO-H")))
842  {
843  result = writeRegister(id, "Goal_Velocity", velocity, log);
844  result = writeRegister(id, "Goal_Acceleration", acceleration, log);
845  }
846  else
847  {
848  result = writeRegister(id, "Profile_Acceleration", acceleration, log);
849  result = writeRegister(id, "Profile_Velocity", velocity, log);
850  }
851  }
852 
853  if (result == false)
854  {
855  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Joint Mode!";
856  return false;
857  }
858 
859  result = torqueOn(id, log);
860  if (result == false) return false;
861 
862  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Joint Mode!";
863  return result;
864 }
865 
866 bool DynamixelWorkbench::wheelMode(uint8_t id, int32_t acceleration, const char **log)
867 {
868  bool result = false;
869 
870  model_name = getModelName(id, log);
871  if (model_name == NULL) return false;
872 
873  result = torqueOff(id, log);
874  if (result == false) return false;
875 
876  result = setVelocityControlMode(id, log);
877  if (result == false) return false;
878 
879  if (getProtocolVersion() == 1.0)
880  {
881  if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) ||
882  !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) ||
883  !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) ||
884  !strncmp(model_name, "XL430", strlen("XL430")) ||
885  !strncmp(model_name, "XC430", strlen("XC430")) ||
886  !strncmp(model_name, "XM", strlen("XM")) ||
887  !strncmp(model_name, "XH", strlen("XH")) ||
888  !strncmp(model_name, "XW", strlen("XW")))
889  {
890  result = writeRegister(id, "Profile_Acceleration", acceleration, log);
891  }
892  else if (!strncmp(model_name, "MX-28", strlen("MX-28")) ||
893  !strncmp(model_name, "MX-64", strlen("MX-64")) ||
894  !strncmp(model_name, "MX-106", strlen("MX-106")))
895  {
896  result = writeRegister(id, "Goal_Acceleration", acceleration, log);
897  }
898  }
899  else if (getProtocolVersion() == 2.0)
900  {
901  if (!strncmp(model_name, "PRO-PLUS", strlen("PRO-PLUS")))
902  {
903  result = writeRegister(id, "Profile_Acceleration", acceleration, log);
904  }
905  else if (!strncmp(model_name, "PRO", strlen("PRO")))
906  {
907  result = writeRegister(id, "Goal_Acceleration", acceleration, log);
908  }
909  else
910  {
911  result = writeRegister(id, "Profile_Acceleration", acceleration, log);
912  }
913  }
914 
915  if (result == false)
916  {
917  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Wheel Mode!";
918  return false;
919  }
920 
921  result = torqueOn(id, log);
922  if (result == false) return false;
923 
924  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Wheel Mode!";
925  return result;
926 }
927 
928 bool DynamixelWorkbench::currentBasedPositionMode(uint8_t id, int32_t current, const char **log)
929 {
930  bool result = false;
931 
932  model_name = getModelName(id, log);
933  if (model_name == NULL) return false;
934 
935  result = torqueOff(id, log);
936  if (result == false) return false;
937 
938  result = setCurrentBasedPositionControlMode(id, log);
939  if (result == false) return false;
940 
941  if (!strncmp(model_name, "MX-64-2", strlen("MX-64-2")) ||
942  !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) ||
943  !strncmp(model_name, "XL330", strlen("XL330")) ||
944  !strncmp(model_name, "XM", strlen("XM")) ||
945  !strncmp(model_name, "XH", strlen("XH")) ||
946  !strncmp(model_name, "XW", strlen("XW")) ||
947  !strncmp(model_name, "RH", strlen("RH")))
948  {
949  result = writeRegister(id, "Goal_Current", current, log);
950  }
951 
952  if (result == false)
953  {
954  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Current Based Position Mode!";
955  return false;
956  }
957 
958  result = torqueOn(id, log);
959  if (result == false) return false;
960 
961  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Current Based Position Wheel Mode!";
962  return result;
963 }
964 
965 //keep compatibility with older codes
966 bool DynamixelWorkbench::goalPosition(uint8_t id, int value, const char **log)
967 {
968 // goalPosition(id, (int32_t)value, log);
969 // }
970 
971 // bool DynamixelWorkbench::goalPosition(uint8_t id, int32_t value, const char **log)
972 // {
973  bool result = false;
974 
975  result = itemWrite(id, "Goal_Position", value, log);
976 
977  if (result == false)
978  {
979  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set goal position!";
980  return false;
981  }
982 
983  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal position!";
984  return result;
985 }
986 
987 //keep compatibility with older codes
988 bool DynamixelWorkbench::goalSpeed(uint8_t id, int value, const char **log)
989 {
990  bool result = false;
991  result = goalVelocity(id, (int32_t)value, log);
992  return result;
993 }
994 
995 //keep compatibility with older codes
996 bool DynamixelWorkbench::goalVelocity(uint8_t id, int value, const char **log)
997 {
998 // goalVelocity(id, (int32_t)value, log);
999 // }
1000 
1001 // bool DynamixelWorkbench::goalVelocity(uint8_t id, int32_t value, const char **log)
1002 // {
1003  bool result[2] = {false, false};
1004 
1005  if (getProtocolVersion() == 2.0f)
1006  {
1007  result[0] = writeRegister(id, "Goal_Velocity", value, log);
1008  if (result[0] == false)
1009  {
1010  if (value < 0)
1011  {
1012  value = (-1) * value;
1013  value |= 1024;
1014  }
1015  result[1] = writeRegister(id, "Moving_Speed", value, log);
1016  if (result[1] == false)
1017  {
1018  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set goal velocity!";
1019  return false;
1020  }
1021  else
1022  {
1023  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal velocity!";
1024  return true;
1025  }
1026  }
1027  else
1028  {
1029  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal velocity!";
1030  return true;
1031  }
1032  }
1033  else
1034  {
1035  result[0] = writeRegister(id, "Goal_Velocity", value, log);
1036  if (result[0] == false)
1037  {
1038  if (value < 0)
1039  {
1040  value = (-1) * value;
1041  value |= 1024;
1042  }
1043  result[1] = writeRegister(id, "Moving_Speed", value, log);
1044  if (result[1] == false)
1045  {
1046  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set goal velocity!";
1047  return false;
1048  }
1049  else
1050  {
1051  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal velocity!";
1052  return true;
1053  }
1054  }
1055  else
1056  {
1057  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal velocity!";
1058  return true;
1059  }
1060  }
1061 
1062  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set goal velocity!";
1063  return false;
1064 }
1065 
1066 bool DynamixelWorkbench::goalPosition(uint8_t id, float radian, const char **log)
1067 {
1068  bool result = 0;
1069  uint32_t value = 0;
1070 
1071  value = convertRadian2Value(id, radian);
1072 
1073  result = goalPosition(id, (int32_t)value, log);
1074  if (result == false)
1075  {
1076  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set goal position!";
1077  return false;
1078  }
1079 
1080  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal position!";
1081  return true;
1082 }
1083 
1084 bool DynamixelWorkbench::goalVelocity(uint8_t id, float velocity, const char **log)
1085 {
1086  bool result = 0;
1087  int32_t value = 0;
1088 
1089  value = convertVelocity2Value(id, velocity);
1090 
1091  result = goalVelocity(id, value, log);
1092  if (result == false)
1093  {
1094  if (log != NULL) *log = "[DynamixelWorkbench] Failed to set goal velocity!";
1095  return result;
1096  }
1097 
1098  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal velocity!";
1099  return result;
1100 }
1101 
1102 bool DynamixelWorkbench::getPresentPositionData(uint8_t id, int32_t* data, const char **log)
1103 {
1104  bool result = 0;
1105  int32_t get_data = 0;
1106 
1107  result = readRegister(id, "Present_Position", &get_data, log);
1108  if (result == false)
1109  {
1110  if (log != NULL) *log = "[DynamixelWorkbench] Failed to get present position data!";
1111  return result;
1112  }
1113 
1114  *data = get_data;
1115 
1116  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to get present position data!";
1117  return result;
1118 }
1119 
1120 bool DynamixelWorkbench::getRadian(uint8_t id, float* radian, const char **log)
1121 {
1122  bool result = 0;
1123  int32_t get_data = 0;
1124 
1125  result = getPresentPositionData(id, &get_data, log);
1126  if (result == false)
1127  {
1128  if (log != NULL) *log = "[DynamixelWorkbench] Failed to get radian!";
1129  return result;
1130  }
1131 
1132  *radian = convertValue2Radian(id, get_data);
1133 
1134  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to get radian!";
1135  return result;
1136 }
1137 
1138 bool DynamixelWorkbench::getVelocity(uint8_t id, float* velocity, const char **log)
1139 {
1140  bool result = 0;
1141  int32_t get_data = 0;
1142 
1143  result = getPresentVelocityData(id, &get_data, log);
1144  if (result == false)
1145  {
1146  if (log != NULL) *log = "[DynamixelWorkbench] Failed to get velocity!";
1147  return result;
1148  }
1149 
1150  *velocity = convertValue2Velocity(id, get_data);
1151 
1152  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to get velocity!";
1153  return result;
1154 }
1155 
1156 bool DynamixelWorkbench::getPresentVelocityData(uint8_t id, int32_t* data, const char **log)
1157 {
1158  bool result = 0;
1159  int32_t get_data = 0;
1160 
1161  result = readRegister(id, "Present_Speed", &get_data, log);
1162  if (result == false)
1163  {
1164  if (log != NULL) *log = "[DynamixelWorkbench] Failed to get present speed data!";
1165  return result;
1166  }
1167 
1168  *data = get_data;
1169  if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to get present speed data!";
1170  return result;
1171 }
1172 
1173 int32_t DynamixelWorkbench::convertRadian2Value(uint8_t id, float radian)
1174 {
1175  int32_t position = 0;
1176 
1177  model_info = getModelInfo(id);
1178  if (model_info == NULL) return false;
1179 
1180  if (radian > 0)
1181  {
1182  position = (radian * (model_info->value_of_max_radian_position - model_info->value_of_zero_radian_position) / model_info->max_radian) + model_info->value_of_zero_radian_position;
1183  }
1184  else if (radian < 0)
1185  {
1186  position = (radian * (model_info->value_of_min_radian_position - model_info->value_of_zero_radian_position) / model_info->min_radian) + model_info->value_of_zero_radian_position;
1187  }
1188  else
1189  {
1190  position = model_info->value_of_zero_radian_position;
1191  }
1192 
1193  return position;
1194 }
1195 
1196 float DynamixelWorkbench::convertValue2Radian(uint8_t id, int32_t value)
1197 {
1198  float radian = 0.0;
1199 
1200  model_info = getModelInfo(id);
1201  if (model_info == NULL) return false;
1202 
1203  if (value > model_info->value_of_zero_radian_position)
1204  {
1205  radian = (float)(value - model_info->value_of_zero_radian_position) * model_info->max_radian / (float)(model_info->value_of_max_radian_position - model_info->value_of_zero_radian_position);
1206  }
1207  else if (value < model_info->value_of_zero_radian_position)
1208  {
1209  radian = (float)(value - model_info->value_of_zero_radian_position) * model_info->min_radian / (float)(model_info->value_of_min_radian_position - model_info->value_of_zero_radian_position);
1210  }
1211 
1212  return radian;
1213 }
1214 
1215 int32_t DynamixelWorkbench::convertRadian2Value(float radian, int32_t max_position, int32_t min_position, float max_radian, float min_radian)
1216 {
1217  int32_t value = 0;
1218  int32_t zero_position = (max_position + min_position)/2;
1219 
1220  if (radian > 0)
1221  {
1222  value = (radian * (max_position - zero_position) / max_radian) + zero_position;
1223  }
1224  else if (radian < 0)
1225  {
1226  value = (radian * (min_position - zero_position) / min_radian) + zero_position;
1227  }
1228  else
1229  {
1230  value = zero_position;
1231  }
1232 
1233  return value;
1234 }
1235 
1236 float DynamixelWorkbench::convertValue2Radian(int32_t value, int32_t max_position, int32_t min_position, float max_radian, float min_radian)
1237 {
1238  float radian = 0.0;
1239  int32_t zero_position = (max_position + min_position)/2;
1240 
1241  if (value > zero_position)
1242  {
1243  radian = (float)(value - zero_position) * max_radian / (float)(max_position - zero_position);
1244  }
1245  else if (value < zero_position)
1246  {
1247  radian = (float)(value - zero_position) * min_radian / (float)(min_position - zero_position);
1248  }
1249 
1250  return radian;
1251 }
1252 
1253 int32_t DynamixelWorkbench::convertVelocity2Value(uint8_t id, float velocity)
1254 {
1255  int32_t value = 0;
1256  const float RPM2RADPERSEC = 0.104719755f;
1257 
1258  model_info = getModelInfo(id);
1259  if (model_info == NULL) return false;
1260 
1261  if (getProtocolVersion() == 1.0f)
1262  {
1263  if (strncmp(getModelName(id), "AX", strlen("AX")) == 0 ||
1264  strncmp(getModelName(id), "RX", strlen("RX")) == 0 ||
1265  strncmp(getModelName(id), "EX", strlen("EX")) == 0 ||
1266  strncmp(getModelName(id), "MX", strlen("MX")) == 0)
1267  {
1268  if (velocity == 0.0f) value = 0;
1269  else if (velocity < 0.0f) value = (velocity / (model_info->rpm * RPM2RADPERSEC));
1270  else if (velocity > 0.0f) value = (velocity / (model_info->rpm * RPM2RADPERSEC)) + 1023;
1271 
1272  return value;
1273  }
1274  }
1275  else if (getProtocolVersion() == 2.0f)
1276  {
1277  if (strcmp(getModelName(id), "XL-320") == 0)
1278  {
1279  if (velocity == 0.0f) value = 0;
1280  else if (velocity < 0.0f) value = (velocity / (model_info->rpm * RPM2RADPERSEC));
1281  else if (velocity > 0.0f) value = (velocity / (model_info->rpm * RPM2RADPERSEC)) + 1023;
1282 
1283  return value;
1284  }
1285  else
1286  {
1287  value = velocity / (model_info->rpm * RPM2RADPERSEC);
1288  }
1289 
1290  return value;
1291  }
1292 
1293  return 0;
1294 }
1295 
1296 float DynamixelWorkbench::convertValue2Velocity(uint8_t id, int32_t value)
1297 {
1298  float velocity = 0;
1299  const float RPM2RADPERSEC = 0.104719755f;
1300 
1301  model_info = getModelInfo(id);
1302  if (model_info == NULL) return false;
1303 
1304  if (getProtocolVersion() == 1.0f)
1305  {
1306  if (strncmp(getModelName(id), "AX", strlen("AX")) == 0 ||
1307  strncmp(getModelName(id), "RX", strlen("RX")) == 0 ||
1308  strncmp(getModelName(id), "EX", strlen("EX")) == 0 ||
1309  strncmp(getModelName(id), "MX", strlen("MX")) == 0)
1310  {
1311  if (value == 1023 || value == 0) velocity = 0.0f;
1312  else if (value > 0 && value < 1023) velocity = value * model_info->rpm * RPM2RADPERSEC;
1313  else if (value > 1023 && value < 2048) velocity = (value - 1023) * model_info->rpm * RPM2RADPERSEC * (-1.0f);
1314 
1315  return velocity;
1316  }
1317  }
1318  else if (getProtocolVersion() == 2.0f)
1319  {
1320  if (strcmp(getModelName(id), "XL-320") == 0)
1321  {
1322  if (value == 1023 || value == 0) velocity = 0.0f;
1323  else if (value > 0 && value < 1023) velocity = value * model_info->rpm * RPM2RADPERSEC;
1324  else if (value > 1023 && value < 2048) velocity = (value - 1023) * model_info->rpm * RPM2RADPERSEC * (-1.0f);
1325  }
1326  else
1327  {
1328  velocity = value * (model_info->rpm * RPM2RADPERSEC);
1329  }
1330 
1331  return velocity;
1332  }
1333 
1334  return 0.0f;
1335 }
1336 
1337 int16_t DynamixelWorkbench::convertCurrent2Value(uint8_t id, float current)
1338 {
1339  float CURRENT_UNIT = 2.69f; //Unit : mA, Ref : http://emanual.robotis.com/docs/en/dxl/x/xm430-w350/#goal-current102
1340 
1341  model_info = getModelInfo(id);
1342  if (model_info == NULL) return false;
1343 
1344  if (getProtocolVersion() == 1.0f)
1345  {
1346  return (current / CURRENT_UNIT);
1347  }
1348  else if (getProtocolVersion() == 2.0f)
1349  {
1350  if (strncmp(getModelName(id), "PRO-L", strlen("PRO-L")) == 0 ||
1351  strncmp(getModelName(id), "PRO-M", strlen("PRO-M")) == 0 ||
1352  strncmp(getModelName(id), "PRO-H", strlen("PRO-H")) == 0)
1353  {
1354  CURRENT_UNIT = 16.11328f;
1355  return (current / CURRENT_UNIT);
1356  }
1357  else if (strncmp(getModelName(id), "PRO-PLUS", strlen("PRO-PLUS")) == 0)
1358  {
1359  CURRENT_UNIT = 1.0f;
1360  return (current / CURRENT_UNIT);
1361  }
1362  else
1363  {
1364  return (current / CURRENT_UNIT);
1365  }
1366  }
1367 
1368  return (current / CURRENT_UNIT);
1369 }
1370 
1371 int16_t DynamixelWorkbench::convertCurrent2Value(float current)
1372 {
1373  int16_t value = 0;
1374  const float CURRENT_UNIT = 2.69f; //Unit : mA, Ref : http://emanual.robotis.com/docs/en/dxl/x/xm430-w350/#goal-current102
1375 
1376  value = current / CURRENT_UNIT;
1377 
1378  return value;
1379 }
1380 
1381 float DynamixelWorkbench::convertValue2Current(uint8_t id, int16_t value)
1382 {
1383  float current = 0;
1384  float CURRENT_UNIT = 2.69f; //Unit : mA, Ref : http://emanual.robotis.com/docs/en/dxl/x/xm430-w350/#goal-current102
1385 
1386  model_info = getModelInfo(id);
1387  if (model_info == NULL) return false;
1388 
1389  if (getProtocolVersion() == 1.0f)
1390  {
1391  current = (int16_t)value * CURRENT_UNIT;
1392  return current;
1393  }
1394  else if (getProtocolVersion() == 2.0f)
1395  {
1396  if (strncmp(getModelName(id), "PRO-L", strlen("PRO-L")) == 0 ||
1397  strncmp(getModelName(id), "PRO-M", strlen("PRO-M")) == 0 ||
1398  strncmp(getModelName(id), "PRO-H", strlen("PRO-H")) == 0)
1399  {
1400  CURRENT_UNIT = 16.11328f;
1401  current = (int16_t)value * CURRENT_UNIT;
1402  return current;
1403  }
1404  else if (strncmp(getModelName(id), "PRO-PLUS", strlen("PRO-PLUS")) == 0)
1405  {
1406  CURRENT_UNIT = 1.0f;
1407  current = (int16_t)value * CURRENT_UNIT;
1408  return current;
1409  }
1410  else
1411  {
1412  current = (int16_t)value * CURRENT_UNIT;
1413  return current;
1414  }
1415  }
1416 
1417  current = (int16_t)value * CURRENT_UNIT;
1418 
1419  return current;
1420 }
1421 
1422 float DynamixelWorkbench::convertValue2Current(int16_t value)
1423 {
1424  float current = 0;
1425  const float CURRENT_UNIT = 2.69f; //Unit : mA, Ref : http://emanual.robotis.com/docs/en/dxl/x/xm430-w350/#goal-current102
1426 
1427  current = (int16_t)value * CURRENT_UNIT;
1428 
1429  return current;
1430 }
1431 
1432 float DynamixelWorkbench::convertValue2Load(int16_t value)
1433 {
1434  float load = 0;
1435  const float LOAD_UNIT = 0.1f; //Unit : %, Ref : http://emanual.robotis.com/docs/en/dxl/mx/mx-28/#present-load
1436 
1437  if (value == 1023 || value == 0) load = 0.0f;
1438  else if (value > 0 && value < 1023) load = value * LOAD_UNIT;
1439  else if (value > 1023 && value < 2048) load = (value - 1023) * LOAD_UNIT * (-1.0f);
1440 
1441  return load;
1442 }
DynamixelWorkbench::setCurrentBasedPositionControlMode
bool setCurrentBasedPositionControlMode(uint8_t id, const char **log=NULL)
Definition: dynamixel_workbench.cpp:554
DynamixelWorkbench::wheelMode
bool wheelMode(uint8_t id, int32_t acceleration=0, const char **log=NULL)
Definition: dynamixel_workbench.cpp:866
DynamixelDriver::writeRegister
bool writeRegister(uint8_t id, uint16_t address, uint16_t length, uint8_t *data, const char **log=NULL)
Definition: dynamixel_driver.cpp:569
DynamixelWorkbench::setTimeBasedProfile
bool setTimeBasedProfile(uint8_t id, const char **log=NULL)
Definition: dynamixel_workbench.cpp:390
DynamixelWorkbench::convertValue2Load
float convertValue2Load(int16_t value)
Definition: dynamixel_workbench.cpp:1432
DynamixelWorkbench::torque
bool torque(uint8_t id, int32_t onoff, const char **log=NULL)
Definition: dynamixel_workbench.cpp:40
DynamixelDriver::setPacketHandler
bool setPacketHandler(float protocol_version, const char **log=NULL)
Definition: dynamixel_driver.cpp:153
DynamixelWorkbench::getRadian
bool getRadian(uint8_t id, float *radian, const char **log=NULL)
Definition: dynamixel_workbench.cpp:1120
DynamixelWorkbench::changeID
bool changeID(uint8_t id, uint8_t new_id, const char **log=NULL)
Definition: dynamixel_workbench.cpp:73
DynamixelWorkbench::convertValue2Current
float convertValue2Current(uint8_t id, int16_t value)
Definition: dynamixel_workbench.cpp:1381
DynamixelWorkbench::goalPosition
bool goalPosition(uint8_t id, int value, const char **log=NULL)
Definition: dynamixel_workbench.cpp:966
DynamixelWorkbench::currentBasedPositionMode
bool currentBasedPositionMode(uint8_t id, int32_t current=0, const char **log=NULL)
Definition: dynamixel_workbench.cpp:928
ModelInfo::value_of_min_radian_position
int64_t value_of_min_radian_position
Definition: dynamixel_item.h:125
DynamixelWorkbench::itemWrite
bool itemWrite(uint8_t id, const char *item_name, int32_t data, const char **log=NULL)
Definition: dynamixel_workbench.cpp:248
DynamixelWorkbench::getPresentVelocityData
bool getPresentVelocityData(uint8_t id, int32_t *data, const char **log=NULL)
Definition: dynamixel_workbench.cpp:1156
DynamixelWorkbench::setReverseDirection
bool setReverseDirection(uint8_t id, const char **log=NULL)
Definition: dynamixel_workbench.cpp:324
DynamixelWorkbench::goalVelocity
bool goalVelocity(uint8_t id, int value, const char **log=NULL)
Definition: dynamixel_workbench.cpp:996
DynamixelWorkbench::convertValue2Radian
float convertValue2Radian(uint8_t id, int32_t value)
Definition: dynamixel_workbench.cpp:1196
DynamixelWorkbench::~DynamixelWorkbench
~DynamixelWorkbench()
Definition: dynamixel_workbench.cpp:38
DynamixelWorkbench::led
bool led(uint8_t id, int32_t onoff, const char **log=NULL)
Definition: dynamixel_workbench.cpp:258
TORQUE_CONTROL_MODE
static const uint8_t TORQUE_CONTROL_MODE
Definition: dynamixel_workbench.cpp:30
DynamixelWorkbench::setMultiTurnControlMode
bool setMultiTurnControlMode(uint8_t id, const char **log=NULL)
Definition: dynamixel_workbench.cpp:538
CURRENT_CONTROL_MODE
static const uint8_t CURRENT_CONTROL_MODE
Definition: dynamixel_workbench.cpp:24
DynamixelWorkbench::getPresentPositionData
bool getPresentPositionData(uint8_t id, int32_t *data, const char **log=NULL)
Definition: dynamixel_workbench.cpp:1102
ModelInfo::min_radian
float min_radian
Definition: dynamixel_item.h:129
DynamixelWorkbench::setOperatingMode
bool setOperatingMode(uint8_t id, uint8_t index, const char **log=NULL)
Definition: dynamixel_workbench.cpp:586
f
f
DynamixelWorkbench::goalSpeed
bool goalSpeed(uint8_t id, int value, const char **log=NULL)
Definition: dynamixel_workbench.cpp:988
DynamixelDriver::getModelInfo
const ModelInfo * getModelInfo(uint8_t id, const char **log=NULL)
Definition: dynamixel_driver.cpp:232
DynamixelWorkbench::setVelocityBasedProfile
bool setVelocityBasedProfile(uint8_t id, const char **log=NULL)
Definition: dynamixel_workbench.cpp:357
ModelInfo::max_radian
float max_radian
Definition: dynamixel_item.h:130
EXTENDED_POSITION_CONTROL_MODE
static const uint8_t EXTENDED_POSITION_CONTROL_MODE
Definition: dynamixel_workbench.cpp:27
POSITION_CONTROL_MODE
static const uint8_t POSITION_CONTROL_MODE
Definition: dynamixel_workbench.cpp:26
MULTI_TURN_MODE
static const uint8_t MULTI_TURN_MODE
Definition: dynamixel_workbench.cpp:31
model_name
static const char * model_name
Definition: dynamixel_workbench.cpp:33
DynamixelWorkbench::ledOn
bool ledOn(uint8_t id, const char **log=NULL)
Definition: dynamixel_workbench.cpp:273
DynamixelWorkbench::convertVelocity2Value
int32_t convertVelocity2Value(uint8_t id, float velocity)
Definition: dynamixel_workbench.cpp:1253
DynamixelWorkbench::torqueOn
bool torqueOn(uint8_t id, const char **log=NULL)
Definition: dynamixel_workbench.cpp:55
ModelInfo
Definition: dynamixel_item.h:121
PWM_CONTROL_MODE
static const uint8_t PWM_CONTROL_MODE
Definition: dynamixel_workbench.cpp:29
ModelInfo::value_of_max_radian_position
int64_t value_of_max_radian_position
Definition: dynamixel_item.h:127
JOINT_MODE
static const uint8_t JOINT_MODE
Definition: dynamixel_workbench.cpp:22
DynamixelWorkbench::convertValue2Velocity
float convertValue2Velocity(uint8_t id, int32_t value)
Definition: dynamixel_workbench.cpp:1296
DynamixelWorkbench::setSecondaryID
bool setSecondaryID(uint8_t id, uint8_t secondary_id, const char **log=NULL)
Definition: dynamixel_workbench.cpp:423
DynamixelDriver::getProtocolVersion
float getProtocolVersion(void)
Definition: dynamixel_driver.cpp:167
CURRENT_BASED_POSITION_CONTROL_MODE
static const uint8_t CURRENT_BASED_POSITION_CONTROL_MODE
Definition: dynamixel_workbench.cpp:28
DynamixelWorkbench::convertCurrent2Value
int16_t convertCurrent2Value(uint8_t id, float current)
Definition: dynamixel_workbench.cpp:1337
WHEEL_MODE
static const uint8_t WHEEL_MODE
Definition: dynamixel_workbench.cpp:21
ModelInfo::rpm
float rpm
Definition: dynamixel_item.h:123
model_info
static const ModelInfo * model_info
Definition: dynamixel_workbench.cpp:34
DynamixelWorkbench::setVelocityControlMode
bool setVelocityControlMode(uint8_t id, const char **log=NULL)
Definition: dynamixel_workbench.cpp:474
DynamixelWorkbench::convertRadian2Value
int32_t convertRadian2Value(uint8_t id, float radian)
Definition: dynamixel_workbench.cpp:1173
DynamixelWorkbench::setTorqueControlMode
bool setTorqueControlMode(uint8_t id, const char **log=NULL)
Definition: dynamixel_workbench.cpp:506
DynamixelWorkbench::setExtendedPositionControlMode
bool setExtendedPositionControlMode(uint8_t id, const char **log=NULL)
Definition: dynamixel_workbench.cpp:522
ModelInfo::value_of_zero_radian_position
int64_t value_of_zero_radian_position
Definition: dynamixel_item.h:126
DynamixelWorkbench::itemRead
bool itemRead(uint8_t id, const char *item_name, int32_t *data, const char **log=NULL)
Definition: dynamixel_workbench.cpp:253
DynamixelWorkbench::setPositionControlMode
bool setPositionControlMode(uint8_t id, const char **log=NULL)
Definition: dynamixel_workbench.cpp:458
DynamixelDriver::getModelName
const char * getModelName(uint8_t id, const char **log=NULL)
Definition: dynamixel_driver.cpp:177
DynamixelWorkbench::ledOff
bool ledOff(uint8_t id, const char **log=NULL)
Definition: dynamixel_workbench.cpp:282
DynamixelWorkbench::setNormalDirection
bool setNormalDirection(uint8_t id, const char **log=NULL)
Definition: dynamixel_workbench.cpp:291
DynamixelWorkbench::DynamixelWorkbench
DynamixelWorkbench()
Definition: dynamixel_workbench.cpp:36
DynamixelWorkbench::changeProtocolVersion
bool changeProtocolVersion(uint8_t id, uint8_t version, const char **log=NULL)
Definition: dynamixel_workbench.cpp:217
VELOCITY_CONTROL_MODE
static const uint8_t VELOCITY_CONTROL_MODE
Definition: dynamixel_workbench.cpp:25
DynamixelWorkbench::getVelocity
bool getVelocity(uint8_t id, float *velocity, const char **log=NULL)
Definition: dynamixel_workbench.cpp:1138
DynamixelWorkbench::setPWMControlMode
bool setPWMControlMode(uint8_t id, const char **log=NULL)
Definition: dynamixel_workbench.cpp:570
DynamixelWorkbench::changeBaudrate
bool changeBaudrate(uint8_t id, uint32_t new_baudrate, const char **log=NULL)
Definition: dynamixel_workbench.cpp:92
DynamixelWorkbench::setCurrentControlMode
bool setCurrentControlMode(uint8_t id, const char **log=NULL)
Definition: dynamixel_workbench.cpp:490
DynamixelDriver::readRegister
bool readRegister(uint8_t id, uint16_t address, uint16_t length, uint32_t *data, const char **log=NULL)
Definition: dynamixel_driver.cpp:772
DynamixelWorkbench::torqueOff
bool torqueOff(uint8_t id, const char **log=NULL)
Definition: dynamixel_workbench.cpp:64
DynamixelWorkbench::jointMode
bool jointMode(uint8_t id, int32_t velocity=0, int32_t acceleration=0, const char **log=NULL)
Definition: dynamixel_workbench.cpp:783

dynamixel_workbench_toolbox
Author(s): Darby Lim , Ryan Shim
autogenerated on Wed Mar 2 2022 00:13:18