dxl_driver.hpp
Go to the documentation of this file.
1 /*
2 dxl_driver.hpp
3 Copyright (C) 2020 Niryo
4 All rights reserved.
5 This program is free software: you can redistribute it and/or modify
6 it under the terms of the GNU General Public License as published by
7 the Free Software Foundation, either version 3 of the License, or
8 (at your option) any later version.
9 This program is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU General Public License for more details.
13 You should have received a copy of the GNU General Public License
14 along with this program. If not, see <http:// www.gnu.org/licenses/>.
15 */
16 
17 #ifndef DXL_DRIVER_HPP
18 #define DXL_DRIVER_HPP
19 
20 #include <memory>
21 #include <vector>
22 #include <string>
23 #include <iostream>
24 #include <sstream>
25 #include <cassert>
26 
27 #include "abstract_dxl_driver.hpp"
28 #include "common/common_defs.hpp"
29 
30 #include "xc430_reg.hpp"
31 #include "xl430_reg.hpp"
32 #include "xm430_reg.hpp"
33 #include "xl330_reg.hpp"
34 #include "xl320_reg.hpp"
35 #include "xh430_reg.hpp"
36 
37 namespace ttl_driver
38 {
39 
43  template <typename reg_type>
44  class DxlDriver : public AbstractDxlDriver
45  {
46  public:
47  DxlDriver(std::shared_ptr<dynamixel::PortHandler> portHandler,
48  std::shared_ptr<dynamixel::PacketHandler> packetHandler);
49 
50  std::string str() const override;
51  std::string interpretErrorState(uint32_t hw_state) const override;
52 
53  public:
54  int checkModelNumber(uint8_t id, uint16_t& model_number) override;
55  int readFirmwareVersion(uint8_t id, std::string &version) override;
56 
57  int readTemperature(uint8_t id, uint8_t &temperature) override;
58  int readVoltage(uint8_t id, double &voltage) override;
59  int readHwErrorStatus(uint8_t id, uint8_t &hardware_error_status) override;
60 
61  int syncReadFirmwareVersion(const std::vector<uint8_t> &id_list, std::vector<std::string> &firmware_list) override;
62  int syncReadTemperature(const std::vector<uint8_t> &id_list, std::vector<uint8_t> &temperature_list) override;
63  int syncReadVoltage(const std::vector<uint8_t> &id_list, std::vector<double> &voltage_list) override;
64  int syncReadRawVoltage(const std::vector<uint8_t> &id_list, std::vector<double> &voltage_list) override;
65  int syncReadHwStatus(const std::vector<uint8_t> &id_list, std::vector<std::pair<double, uint8_t>> &data_list) override;
66 
67  int syncReadHwErrorStatus(const std::vector<uint8_t> &id_list, std::vector<uint8_t> &hw_error_list) override;
68 
69  protected:
70  // AbstractTtlDriver interface
71  std::string interpretFirmwareVersion(uint32_t fw_version) const override;
72 
73  public:
74  // AbstractMotorDriver interface : we cannot define them globally in AbstractMotorDriver
75  // as it is needed here for polymorphism (AbstractMotorDriver cannot be a template class and does not
76  // have access to reg_type). So it seems like a duplicate of StepperDriver
77 
78  // eeprom write
79  int changeId(uint8_t id, uint8_t new_id) override;
80 
81  // eeprom read
82  int readMinPosition(uint8_t id, uint32_t &min_pos) override;
83  int readMaxPosition(uint8_t id, uint32_t &max_pos) override;
84 
85  // ram write
86  int writeVelocityProfile(uint8_t id, const std::vector<uint32_t> &data_list) override;
87 
88  int writeTorquePercentage(uint8_t id, uint8_t torque_percentage) override;
89 
90  int writePositionGoal(uint8_t id, uint32_t position) override;
91  int writeVelocityGoal(uint8_t id, uint32_t velocity) override;
92 
93  int syncWriteTorquePercentage(const std::vector<uint8_t> &id_list, const std::vector<uint8_t> &torque_percentage_list) override;
94  int syncWritePositionGoal(const std::vector<uint8_t> &id_list, const std::vector<uint32_t> &position_list) override;
95  int syncWriteVelocityGoal(const std::vector<uint8_t> &id_list, const std::vector<uint32_t> &velocity_list) override;
96 
97  // ram read
98  int readVelocityProfile(uint8_t id, std::vector<uint32_t> &data_list) override;
99 
100  int readPosition(uint8_t id, uint32_t &present_position) override;
101  int readVelocity(uint8_t id, uint32_t &present_velocity) override;
102 
103  int syncReadPosition(const std::vector<uint8_t> &id_list, std::vector<uint32_t> &position_list) override;
104  int syncReadVelocity(const std::vector<uint8_t> &id_list, std::vector<uint32_t> &velocity_list) override;
105  int syncReadJointStatus(const std::vector<uint8_t> &id_list, std::vector<std::array<uint32_t, 2>> &data_array_list) override;
106 
107  public:
108  // AbstractDxlDriver interface
109 
110  // eeprom read
111 
112  // eeprom write
113  int writeStartupConfiguration(uint8_t id, uint8_t config) override;
114  int writeTemperatureLimit(uint8_t id, uint8_t temperature_limit) override;
115  int writeShutdownConfiguration(uint8_t id, uint8_t configuration) override;
116 
117  // ram read
118  int readLoad(uint8_t id, uint16_t &present_load) override;
119  int syncReadLoad(const std::vector<uint8_t> &id_list, std::vector<uint16_t> &load_list) override;
120 
121  int readPID(uint8_t id, std::vector<uint16_t> &data_list) override;
122  int readControlMode(uint8_t id, uint8_t &control_mode) override;
123 
124  int readMoving(uint8_t id, uint8_t &status) override;
125 
126  // ram write
127  int writePID(uint8_t id, const std::vector<uint16_t> &data) override;
128  int writeControlMode(uint8_t id, uint8_t control_mode) override;
129 
130  int writeLed(uint8_t id, uint8_t led_value) override;
131  int syncWriteLed(const std::vector<uint8_t> &id_list, const std::vector<uint8_t> &led_list) override;
132 
133  int writeTorqueGoal(uint8_t id, uint16_t torque) override;
134  int syncWriteTorqueGoal(const std::vector<uint8_t> &id_list, const std::vector<uint16_t> &torque_list) override;
135  };
136 
137  // definition of methods
138 
142  template <typename reg_type>
143  DxlDriver<reg_type>::DxlDriver(std::shared_ptr<dynamixel::PortHandler> portHandler,
144  std::shared_ptr<dynamixel::PacketHandler> packetHandler) : AbstractDxlDriver(std::move(portHandler),
145  std::move(packetHandler))
146  {
147  }
148 
149  //*****************************
150  // AbstractMotorDriver interface
151  //*****************************
152 
157  template <typename reg_type>
158  std::string DxlDriver<reg_type>::str() const
159  {
160  return common::model::HardwareTypeEnum(reg_type::motor_type).toString() + " : " + AbstractDxlDriver::str();
161  }
162 
167  template <typename reg_type>
168  std::string DxlDriver<reg_type>::interpretErrorState(uint32_t /*hw_state*/) const
169  {
170  return "";
171  }
172 
178  template <typename reg_type>
179  std::string DxlDriver<reg_type>::interpretFirmwareVersion(uint32_t fw_version) const
180  {
181  std::string version = std::to_string(static_cast<uint8_t>(fw_version));
182 
183  return version;
184  }
185 
192  template <typename reg_type>
193  int DxlDriver<reg_type>::changeId(uint8_t id, uint8_t new_id)
194  {
195  return write<typename reg_type::TYPE_ID>(reg_type::ADDR_ID, id, new_id);
196  }
197 
203  template <typename reg_type>
204  int DxlDriver<reg_type>::checkModelNumber(uint8_t id, uint16_t& model_number)
205  {
206  int ping_result = getModelNumber(id, model_number);
207 
208  if (ping_result == COMM_SUCCESS)
209  {
210  if (model_number && model_number != reg_type::MODEL_NUMBER)
211  {
212  return PING_WRONG_MODEL_NUMBER;
213  }
214  }
215 
216  return ping_result;
217  }
218 
225  template <typename reg_type>
226  int DxlDriver<reg_type>::readFirmwareVersion(uint8_t id, std::string &version)
227  {
228  int res = COMM_RX_FAIL;
229  uint8_t data{};
230  res = read<typename reg_type::TYPE_FIRMWARE_VERSION>(reg_type::ADDR_FIRMWARE_VERSION, id, data);
231  version = interpretFirmwareVersion(data);
232  return res;
233  }
234 
241  template <typename reg_type>
242  int DxlDriver<reg_type>::writeStartupConfiguration(uint8_t id, uint8_t config)
243  {
244  std::string version;
245  int res = readFirmwareVersion(id, version);
246 
247  // only for version above certain version (see registers)
248  if (COMM_SUCCESS == res && std::stoi(version) >= reg_type::VERSION_STARTUP_CONFIGURATION)
249  {
250  res = write<typename reg_type::TYPE_STARTUP_CONFIGURATION>(reg_type::ADDR_STARTUP_CONFIGURATION, id, config);
251  }
252  else
253  {
254  std::cout << "Startup configuration available only for version > " << static_cast<int>(reg_type::VERSION_STARTUP_CONFIGURATION) << std::endl;
255  res = COMM_SUCCESS;
256  }
257 
258  return res;
259  }
260 
267  template <typename reg_type>
268  int DxlDriver<reg_type>::writeTemperatureLimit(uint8_t id, uint8_t temperature_limit)
269  {
270  std::cout << "temperature limit not available for this motor type" << std::endl;
271  return COMM_SUCCESS;
272  }
273 
280  template <typename reg_type>
281  int DxlDriver<reg_type>::writeShutdownConfiguration(uint8_t id, uint8_t configuration)
282  {
283  std::cout << "shutdown configuration not available for this motor type" << std::endl;
284  return COMM_SUCCESS;
285  }
286 
293  template <typename reg_type>
294  int DxlDriver<reg_type>::readMinPosition(uint8_t id, uint32_t &pos)
295  {
296  return read<typename reg_type::TYPE_MIN_POSITION_LIMIT>(reg_type::ADDR_MIN_POSITION_LIMIT, id, pos);
297  }
298 
305  template <typename reg_type>
306  int DxlDriver<reg_type>::readMaxPosition(uint8_t id, uint32_t &pos)
307  {
308  return read<typename reg_type::TYPE_MAX_POSITION_LIMIT>(reg_type::ADDR_MAX_POSITION_LIMIT, id, pos);
309  }
310 
311  // ram write
312 
320  template <typename reg_type>
321  int DxlDriver<reg_type>::writeVelocityProfile(uint8_t id, const std::vector<uint32_t> &data_list)
322  {
323  // in mode control Position Control Mode, velocity profile in datasheet is used to write velocity (except xl320)
324  int res = COMM_RX_FAIL;
325 
326  for (int tries = 10; tries > 0; --tries)
327  {
328  res = write<typename reg_type::TYPE_PROFILE>(reg_type::ADDR_PROFILE_VELOCITY, id, data_list.at(0));
329  if (COMM_SUCCESS == res)
330  break;
331  }
332 
333  for (int tries = 10; tries > 0; --tries)
334  {
335  res = write<typename reg_type::TYPE_PROFILE>(reg_type::ADDR_PROFILE_ACCELERATION, id, data_list.at(1));
336  if (COMM_SUCCESS == res)
337  break;
338  }
339 
340  if (COMM_SUCCESS != res)
341  return res;
342 
343  return res;
344  }
345 
352  template <typename reg_type>
353  int DxlDriver<reg_type>::writeTorquePercentage(uint8_t id, uint8_t torque_percentage)
354  {
355  auto torque_enable = torque_percentage > 0 ? 1 : 0;
356  return write<typename reg_type::TYPE_TORQUE_ENABLE>(reg_type::ADDR_TORQUE_ENABLE, id, torque_enable);
357  }
358 
365  template <typename reg_type>
366  int DxlDriver<reg_type>::writePositionGoal(uint8_t id, uint32_t position)
367  {
368  return write<typename reg_type::TYPE_GOAL_POSITION>(reg_type::ADDR_GOAL_POSITION, id,
369  static_cast<typename reg_type::TYPE_GOAL_POSITION>(position));
370  }
371 
378  template <typename reg_type>
379  int DxlDriver<reg_type>::writeVelocityGoal(uint8_t id, uint32_t velocity)
380  {
381  return write<typename reg_type::TYPE_GOAL_VELOCITY>(reg_type::ADDR_GOAL_VELOCITY, id,
382  static_cast<typename reg_type::TYPE_GOAL_VELOCITY>(velocity));
383  }
384 
391  template <typename reg_type>
392  int DxlDriver<reg_type>::syncWriteTorquePercentage(const std::vector<uint8_t> &id_list, const std::vector<uint8_t> &torque_percentage_list)
393  {
394  std::vector<uint8_t> torque_enable_list;
395  for(const auto &torque_percentage : torque_percentage_list)
396  {
397  auto torque_enable = torque_percentage > 0 ? 1 : 0;
398  torque_enable_list.push_back(torque_enable);
399  }
400  return syncWrite<typename reg_type::TYPE_TORQUE_ENABLE>(reg_type::ADDR_TORQUE_ENABLE, id_list, torque_enable_list);
401  }
402 
409  template <typename reg_type>
410  int DxlDriver<reg_type>::syncWritePositionGoal(const std::vector<uint8_t> &id_list, const std::vector<uint32_t> &position_list)
411  {
412  return syncWrite<typename reg_type::TYPE_GOAL_POSITION>(reg_type::ADDR_GOAL_POSITION, id_list, position_list);
413  }
414 
421  template <typename reg_type>
422  int DxlDriver<reg_type>::syncWriteVelocityGoal(const std::vector<uint8_t> &id_list, const std::vector<uint32_t> &velocity_list)
423  {
424  return syncWrite<typename reg_type::TYPE_GOAL_VELOCITY>(reg_type::ADDR_GOAL_VELOCITY, id_list, velocity_list);
425  }
426 
427  // ram read
428  template <typename reg_type>
429  int DxlDriver<reg_type>::readVelocityProfile(uint8_t id, std::vector<uint32_t> &data_list)
430  {
431  data_list.clear();
432  typename reg_type::TYPE_PROFILE velocity_profile{};
433  typename reg_type::TYPE_PROFILE acceleration_profile{};
434 
435  int res = read<typename reg_type::TYPE_PROFILE>(reg_type::ADDR_PROFILE_VELOCITY, id, velocity_profile);
436  if (COMM_SUCCESS == res)
437  res = read<typename reg_type::TYPE_PROFILE>(reg_type::ADDR_PROFILE_ACCELERATION, id, acceleration_profile);
438 
439  data_list.emplace_back(velocity_profile);
440  data_list.emplace_back(acceleration_profile);
441 
442  return res;
443  }
444 
451  template <typename reg_type>
452  int DxlDriver<reg_type>::readPosition(uint8_t id, uint32_t &present_position)
453  {
454  return read<typename reg_type::TYPE_PRESENT_POSITION>(reg_type::ADDR_PRESENT_POSITION, id, present_position);
455  }
456 
463  template <typename reg_type>
464  int DxlDriver<reg_type>::readTemperature(uint8_t id, uint8_t &temperature)
465  {
466  return read<typename reg_type::TYPE_PRESENT_TEMPERATURE>(reg_type::ADDR_PRESENT_TEMPERATURE, id, temperature);
467  }
468 
475  template <typename reg_type>
476  int DxlDriver<reg_type>::readVoltage(uint8_t id, double &voltage)
477  {
478  typename reg_type::TYPE_PRESENT_VOLTAGE voltage_mV{};
479  int res = read<typename reg_type::TYPE_PRESENT_VOLTAGE>(reg_type::ADDR_PRESENT_VOLTAGE, id, voltage_mV);
480  voltage = static_cast<double>(voltage_mV) / reg_type::VOLTAGE_CONVERSION;
481  return res;
482  }
483 
490  template <typename reg_type>
491  int DxlDriver<reg_type>::readHwErrorStatus(uint8_t id, uint8_t &hardware_error_status)
492  {
493  return read<typename reg_type::TYPE_HW_ERROR_STATUS>(reg_type::ADDR_HW_ERROR_STATUS, id, hardware_error_status);
494  }
495 
502  template <typename reg_type>
503  int DxlDriver<reg_type>::syncReadPosition(const std::vector<uint8_t> &id_list, std::vector<uint32_t> &position_list)
504  {
505  return syncRead<typename reg_type::TYPE_PRESENT_POSITION>(reg_type::ADDR_PRESENT_POSITION, id_list, position_list);
506  }
507 
514  template <typename reg_type>
515  int DxlDriver<reg_type>::writePID(uint8_t id, const std::vector<uint16_t> &data)
516  {
517  int res = COMM_TX_FAIL;
518 
519  // only rewrite params which is not success
520  for (int tries = 10; tries > 0; --tries)
521  {
522  res = write<typename reg_type::TYPE_PID_GAIN>(reg_type::ADDR_POSITION_P_GAIN, id, data.at(0));
523  if (COMM_SUCCESS == res)
524  break;
525  }
526 
527  if (COMM_SUCCESS != res)
528  return res;
529 
530  for (int tries = 10; tries > 0; --tries)
531  {
532  res = write<typename reg_type::TYPE_PID_GAIN>(reg_type::ADDR_POSITION_I_GAIN, id, data.at(1));
533  if (COMM_SUCCESS == res)
534  break;
535  }
536  if (COMM_SUCCESS != res)
537  return res;
538 
539  for (int tries = 10; tries > 0; --tries)
540  {
541  res = write<typename reg_type::TYPE_PID_GAIN>(reg_type::ADDR_POSITION_D_GAIN, id, data.at(2));
542  if (COMM_SUCCESS == res)
543  break;
544  }
545  if (COMM_SUCCESS != res)
546  return res;
547 
548  for (int tries = 10; tries > 0; --tries)
549  {
550  res = write<typename reg_type::TYPE_PID_GAIN>(reg_type::ADDR_VELOCITY_P_GAIN, id, data.at(3));
551  if (COMM_SUCCESS == res)
552  break;
553  }
554  if (COMM_SUCCESS != res)
555  return res;
556 
557  for (int tries = 10; tries > 0; --tries)
558  {
559  res = write<typename reg_type::TYPE_PID_GAIN>(reg_type::ADDR_VELOCITY_I_GAIN, id, data.at(4));
560  if (COMM_SUCCESS == res)
561  break;
562  }
563  if (COMM_SUCCESS != res)
564  return res;
565 
566  for (int tries = 10; tries > 0; --tries)
567  {
568  res = write<typename reg_type::TYPE_PID_GAIN>(reg_type::ADDR_FF1_GAIN, id, data.at(5));
569  if (COMM_SUCCESS == res)
570  break;
571  }
572  if (COMM_SUCCESS != res)
573  return res;
574 
575  for (int tries = 10; tries > 0; --tries)
576  {
577  res = write<typename reg_type::TYPE_PID_GAIN>(reg_type::ADDR_FF2_GAIN, id, data.at(6));
578  if (res == COMM_SUCCESS)
579  break;
580  }
581 
582  return res;
583  }
584 
591  template <typename reg_type>
592  int DxlDriver<reg_type>::syncReadFirmwareVersion(const std::vector<uint8_t> &id_list, std::vector<std::string> &firmware_list)
593  {
594  int res = COMM_RX_FAIL;
595  std::vector<uint8_t> data_list;
596  res = syncRead<typename reg_type::TYPE_FIRMWARE_VERSION>(reg_type::ADDR_FIRMWARE_VERSION, id_list, data_list);
597  for (auto const &data : data_list)
598  firmware_list.emplace_back(interpretFirmwareVersion(data));
599  return res;
600  }
601 
608  template <typename reg_type>
609  int DxlDriver<reg_type>::syncReadTemperature(const std::vector<uint8_t> &id_list, std::vector<uint8_t> &temperature_list)
610  {
611  return syncRead<typename reg_type::TYPE_PRESENT_TEMPERATURE>(reg_type::ADDR_PRESENT_TEMPERATURE, id_list, temperature_list);
612  }
613 
620  template <typename reg_type>
621  int DxlDriver<reg_type>::syncReadVoltage(const std::vector<uint8_t> &id_list, std::vector<double> &voltage_list)
622  {
623  voltage_list.clear();
624  std::vector<typename reg_type::TYPE_PRESENT_VOLTAGE> v_read;
625  int res = syncRead<typename reg_type::TYPE_PRESENT_VOLTAGE>(reg_type::ADDR_PRESENT_VOLTAGE, id_list, v_read);
626  for (auto const &v : v_read)
627  voltage_list.emplace_back(static_cast<double>(v) / reg_type::VOLTAGE_CONVERSION);
628  return res;
629  }
630 
637  template <typename reg_type>
638  int DxlDriver<reg_type>::syncReadRawVoltage(const std::vector<uint8_t> &id_list, std::vector<double> &voltage_list)
639  {
640  voltage_list.clear();
641  std::vector<typename reg_type::TYPE_PRESENT_VOLTAGE> v_read;
642  int res = syncRead<typename reg_type::TYPE_PRESENT_VOLTAGE>(reg_type::ADDR_PRESENT_VOLTAGE, id_list, v_read);
643  for (auto const &v : v_read)
644  voltage_list.emplace_back(static_cast<double>(v));
645  return res;
646  }
647 
654  template <typename reg_type>
655  int DxlDriver<reg_type>::syncReadHwStatus(const std::vector<uint8_t> &id_list,
656  std::vector<std::pair<double, uint8_t>> &data_list)
657  {
658  data_list.clear();
659 
660  std::vector<std::array<uint8_t, 3>> raw_data;
661  int res = syncReadConsecutiveBytes<uint8_t, 3>(reg_type::ADDR_PRESENT_VOLTAGE, id_list, raw_data);
662 
663  for (auto const &data : raw_data)
664  {
665  // Voltage is first reg, uint16
666  auto voltage = static_cast<double>((static_cast<uint16_t>(data.at(1)) << 8) | data.at(0));
667 
668  // Temperature is second reg, uint8
669  uint8_t temperature = data.at(2);
670 
671  data_list.emplace_back(std::make_pair(voltage, temperature));
672  }
673 
674  return res;
675  }
676 
683  template <typename reg_type>
684  int DxlDriver<reg_type>::syncReadHwErrorStatus(const std::vector<uint8_t> &id_list, std::vector<uint8_t> &hw_error_list)
685  {
686  return syncRead<typename reg_type::TYPE_HW_ERROR_STATUS>(reg_type::ADDR_HW_ERROR_STATUS, id_list, hw_error_list);
687  }
688 
689  //*****************************
690  // AbstractDxlDriver interface
691  //*****************************
692 
699  template <typename reg_type>
700  int DxlDriver<reg_type>::writeLed(uint8_t id, uint8_t led_value)
701  {
702  return write<typename reg_type::TYPE_LED>(reg_type::ADDR_LED, id, led_value);
703  }
704 
711  template <typename reg_type>
712  int DxlDriver<reg_type>::syncWriteLed(const std::vector<uint8_t> &id_list, const std::vector<uint8_t> &led_list)
713  {
714  return syncWrite<typename reg_type::TYPE_LED>(reg_type::ADDR_LED, id_list, led_list);
715  }
716 
723  template <typename reg_type>
724  int DxlDriver<reg_type>::writeTorqueGoal(uint8_t id, uint16_t torque)
725  {
726  return write<typename reg_type::TYPE_GOAL_TORQUE>(reg_type::ADDR_GOAL_TORQUE, id, torque);
727  }
728 
735  template <typename reg_type>
736  int DxlDriver<reg_type>::syncWriteTorqueGoal(const std::vector<uint8_t> &id_list, const std::vector<uint16_t> &torque_list)
737  {
738  return syncWrite<typename reg_type::TYPE_GOAL_TORQUE>(reg_type::ADDR_GOAL_TORQUE, id_list, torque_list);
739  }
740 
741  // read
742 
749  template <typename reg_type>
750  int DxlDriver<reg_type>::readPID(uint8_t id, std::vector<uint16_t> &data_list)
751  {
752  int res = 0;
753  data_list.clear();
754  std::vector<std::array<typename reg_type::TYPE_PID_GAIN, 8>> raw_data;
755 
756  // only rewrite params which is not success
757  for (int tries = 10; tries > 0; --tries)
758  {
759  res = syncReadConsecutiveBytes<typename reg_type::TYPE_PID_GAIN, 8>(reg_type::ADDR_VELOCITY_I_GAIN, {id}, raw_data);
760  if (COMM_SUCCESS == res)
761  break;
762  }
763 
764  // we need to reorder the data in its correct place in data_list
765  if (COMM_SUCCESS == res && raw_data.size() == 1)
766  {
767  data_list.emplace_back(raw_data.at(0).at(4)); // pos p is 5th
768  data_list.emplace_back(raw_data.at(0).at(3)); // pos i is 4th
769  data_list.emplace_back(raw_data.at(0).at(2)); // pos d is 3rd
770  data_list.emplace_back(raw_data.at(0).at(1)); // vel p is 2nd
771  data_list.emplace_back(raw_data.at(0).at(0)); // vel i is 1st
772  data_list.emplace_back(raw_data.at(0).at(7)); // ff1 is 8th
773  data_list.emplace_back(raw_data.at(0).at(6)); // ff2 is 7th
774  // nothing at 6th
775  }
776  else
777  {
778  std::cout << "Failures during read PID gains: " << res << std::endl;
779  return COMM_TX_FAIL;
780  }
781 
782  return COMM_SUCCESS;
783  }
784 
791  template <typename reg_type>
792  int DxlDriver<reg_type>::readMoving(uint8_t id, uint8_t &status)
793  {
794  return read<typename reg_type::TYPE_MOVING>(reg_type::ADDR_MOVING, id, status);
795  }
796 
803  template <typename reg_type>
804  int DxlDriver<reg_type>::writeControlMode(uint8_t id, uint8_t control_mode)
805  {
806  int res = COMM_TX_ERROR;
807  uint8_t torque{0};
808  res = read<typename reg_type::TYPE_TORQUE_ENABLE>(reg_type::ADDR_TORQUE_ENABLE, id, torque);
809  if (res == COMM_SUCCESS)
810  {
811  if (torque == 0)
812  {
813  return write<typename reg_type::TYPE_OPERATING_MODE>(reg_type::ADDR_OPERATING_MODE, id, control_mode);
814  }
815  }
816  return res;
817  }
818 
825  template <typename reg_type>
826  int DxlDriver<reg_type>::readControlMode(uint8_t id, uint8_t &control_mode)
827  {
828  int res = 0;
829  typename reg_type::TYPE_OPERATING_MODE raw{};
830  res = read<typename reg_type::TYPE_OPERATING_MODE>(reg_type::ADDR_OPERATING_MODE, id, raw);
831  control_mode = static_cast<uint8_t>(raw);
832  return res;
833  }
834  // other
835 
842  template <typename reg_type>
843  int DxlDriver<reg_type>::readLoad(uint8_t id, uint16_t &present_load)
844  {
845  return read<typename reg_type::TYPE_PRESENT_LOAD>(reg_type::ADDR_PRESENT_LOAD, id, present_load);
846  }
847 
854  template <typename reg_type>
855  int DxlDriver<reg_type>::syncReadLoad(const std::vector<uint8_t> &id_list, std::vector<uint16_t> &load_list)
856  {
857  return syncRead<typename reg_type::TYPE_PRESENT_LOAD>(reg_type::ADDR_PRESENT_LOAD, id_list, load_list);
858  }
859 
866  template <typename reg_type>
867  int DxlDriver<reg_type>::readVelocity(uint8_t id, uint32_t &present_velocity)
868  {
869  return read<typename reg_type::TYPE_PRESENT_VELOCITY>(reg_type::ADDR_PRESENT_VELOCITY, id, present_velocity);
870  }
871 
878  template <typename reg_type>
879  int DxlDriver<reg_type>::syncReadVelocity(const std::vector<uint8_t> &id_list, std::vector<uint32_t> &velocity_list)
880  {
881  return syncRead<typename reg_type::TYPE_PRESENT_VELOCITY>(reg_type::ADDR_PRESENT_VELOCITY, id_list, velocity_list);
882  }
883 
890  template <typename reg_type>
891  int DxlDriver<reg_type>::syncReadJointStatus(const std::vector<uint8_t> &id_list, std::vector<std::array<uint32_t, 2>> &data_array_list)
892  {
893  int res = COMM_TX_FAIL;
894 
895  if (id_list.empty())
896  return res;
897 
898  data_array_list.clear();
899 
900  // read torque enable on first id
901  typename reg_type::TYPE_TORQUE_ENABLE torque{1};
902  res = read<typename reg_type::TYPE_TORQUE_ENABLE>(reg_type::ADDR_TORQUE_ENABLE, id_list.at(0), torque);
903  if (COMM_SUCCESS != res)
904  std::cout << "#############"
905  " ERROR reading dxl torque in syncReadJointStatus (error "
906  << res << ")" << std::endl;
907 
908  // if torque on, read position and velocity
909  if (torque)
910  {
911  res = syncReadConsecutiveBytes<uint32_t, 2>(reg_type::ADDR_PRESENT_VELOCITY, id_list, data_array_list);
912  }
913  else // else read position only
914  {
915  std::vector<uint32_t> position_list;
916  res = syncReadPosition(id_list, position_list);
917  for (auto p : position_list)
918  data_array_list.emplace_back(std::array<uint32_t, 2>{0, p});
919  }
920 
921  return res;
922  }
923 
924  /*
925  * ----------------- specializations --------------------
926  */
927 
928  // XL320
929 
930  template <>
931  inline int DxlDriver<XL320Reg>::writeStartupConfiguration(uint8_t /*id*/, uint8_t /*config*/)
932  {
933  std::cout << "startup configuration for XL320 not available" << std::endl;
934  return COMM_SUCCESS;
935  }
936 
937  template <>
938  inline int DxlDriver<XL320Reg>::readMinPosition(uint8_t /*id*/, uint32_t &pos)
939  {
940  pos = 0;
941  std::cout << "min position hardcoded for motor XL320" << std::endl;
942  return COMM_SUCCESS;
943  }
944 
945  template <>
946  inline int DxlDriver<XL320Reg>::readMaxPosition(uint8_t /*id*/, uint32_t &pos)
947  {
948  pos = 1023;
949  std::cout << "max position hardcoded for motor XL320" << std::endl;
950  return COMM_SUCCESS;
951  }
952 
953  template <>
954  inline std::string DxlDriver<XL320Reg>::interpretErrorState(uint32_t hw_state) const
955  {
956  std::string hardware_message;
957 
958  if (hw_state & 1 << 0) // 0b00000001
959  {
960  hardware_message += "Overload";
961  }
962  if (hw_state & 1 << 1) // 0b00000010
963  {
964  if (!hardware_message.empty())
965  hardware_message += ", ";
966  hardware_message += "OverHeating";
967  }
968  if (hw_state & 1 << 2) // 0b00000100
969  {
970  if (!hardware_message.empty())
971  hardware_message += ", ";
972  hardware_message += "Input voltage out of range";
973  }
974  if (hw_state & 1 << 7) // 0b10000000 => added by us : disconnected error
975  {
976  if (!hardware_message.empty())
977  hardware_message += ", ";
978  hardware_message += "Disconnection";
979  }
980 
981  return hardware_message;
982  }
983 
984  template <>
985  inline int DxlDriver<XL320Reg>::readVelocityProfile(uint8_t /*id*/, std::vector<uint32_t> & /*data_list*/)
986  {
987  std::cout << "readVelocityProfile not available for XL320" << std::endl;
988  return COMM_SUCCESS;
989  }
990 
991  template <>
992  inline int DxlDriver<XL320Reg>::writeVelocityProfile(uint8_t /*id*/, const std::vector<uint32_t> & /*data_list*/)
993  {
994  std::cout << "writeVelocityProfile not available for XL320" << std::endl;
995  return COMM_SUCCESS;
996  }
997 
998  template <>
999  inline int DxlDriver<XL320Reg>::readPosition(uint8_t id, uint32_t &present_position)
1000  {
1001  typename XL320Reg::TYPE_PRESENT_POSITION raw_data{};
1002  int res = read<typename XL320Reg::TYPE_PRESENT_POSITION>(XL320Reg::ADDR_PRESENT_POSITION, id, raw_data);
1003  present_position = raw_data;
1004  return res;
1005  }
1006 
1007  template <>
1008  inline int DxlDriver<XL320Reg>::syncReadPosition(const std::vector<uint8_t> &id_list, std::vector<uint32_t> &position_list)
1009  {
1010  position_list.clear();
1011  std::vector<typename XL320Reg::TYPE_PRESENT_POSITION> raw_data_list{};
1012 
1013  int res = syncRead<typename XL320Reg::TYPE_PRESENT_POSITION>(XL320Reg::ADDR_PRESENT_POSITION, id_list, raw_data_list);
1014  for (auto p : raw_data_list)
1015  position_list.emplace_back(p);
1016 
1017  return res;
1018  }
1019 
1020  template <>
1021  inline int DxlDriver<XL320Reg>::readVelocity(uint8_t id, uint32_t &present_velocity)
1022  {
1023  typename XL320Reg::TYPE_PRESENT_VELOCITY raw_data{};
1024  int res = read<typename XL320Reg::TYPE_PRESENT_VELOCITY>(XL320Reg::ADDR_PRESENT_VELOCITY, id, raw_data);
1025  present_velocity = raw_data;
1026 
1027  return res;
1028  }
1029 
1030  template <>
1031  inline int DxlDriver<XL320Reg>::syncReadVelocity(const std::vector<uint8_t> &id_list, std::vector<uint32_t> &velocity_list)
1032  {
1033  std::vector<typename XL320Reg::TYPE_PRESENT_VELOCITY> raw_data_list;
1034  int res = syncRead<typename XL320Reg::TYPE_PRESENT_VELOCITY>(XL320Reg::ADDR_PRESENT_VELOCITY, id_list, raw_data_list);
1035  for (auto v : raw_data_list)
1036  velocity_list.emplace_back(v);
1037  return res;
1038  }
1039 
1046  template <>
1047  inline int DxlDriver<XL320Reg>::syncReadJointStatus(const std::vector<uint8_t> &id_list, std::vector<std::array<uint32_t, 2>> &data_array_list)
1048  {
1049  int res = COMM_TX_FAIL;
1050 
1051  if (id_list.empty())
1052  return res;
1053 
1054  data_array_list.clear();
1055 
1056  std::vector<uint32_t> position_list;
1057  res = syncReadPosition(id_list, position_list);
1058  if (res == COMM_SUCCESS)
1059  {
1060  for (auto p : position_list)
1061  data_array_list.emplace_back(std::array<uint32_t, 2>{0, p});
1062  }
1063 
1064  return res;
1065  }
1066 
1073  template <>
1074  inline int DxlDriver<XL320Reg>::syncReadHwStatus(const std::vector<uint8_t> &id_list,
1075  std::vector<std::pair<double, uint8_t>> &data_list)
1076  {
1077  data_list.clear();
1078 
1079  std::vector<std::array<uint8_t, 2>> raw_data;
1080  int res = syncReadConsecutiveBytes<uint8_t, 2>(XL320Reg::ADDR_PRESENT_VOLTAGE, id_list, raw_data);
1081 
1082  for (auto const &data : raw_data)
1083  {
1084  // Voltage is first reg, uint16
1085  auto voltage = static_cast<double>(data.at(0));
1086 
1087  // Temperature is second reg, uint8
1088  uint8_t temperature = data.at(1);
1089 
1090  data_list.emplace_back(std::make_pair(voltage, temperature));
1091  }
1092 
1093  return res;
1094  }
1095 
1102  template <>
1103  inline int DxlDriver<XL320Reg>::readPID(uint8_t id, std::vector<uint16_t> &data_list)
1104  {
1105  int res = COMM_RX_FAIL;
1106  data_list.clear();
1107  std::vector<std::array<typename XL320Reg::TYPE_PID_GAIN, 3>> raw_data;
1108 
1109  // only rewrite params which is not success
1110  for (int tries = 10; tries > 0; --tries)
1111  {
1112  res = syncReadConsecutiveBytes<typename XL320Reg::TYPE_PID_GAIN, 3>(XL320Reg::ADDR_POSITION_D_GAIN, {id}, raw_data);
1113  if (COMM_SUCCESS == res)
1114  break;
1115  }
1116 
1117  // we need to reorder the data in its correct place in data_list
1118  if (COMM_SUCCESS == res && raw_data.size() == 1)
1119  {
1120  data_list.emplace_back(static_cast<uint16_t>(raw_data.at(0).at(2))); // pos p is 3rd
1121  data_list.emplace_back(static_cast<uint16_t>(raw_data.at(0).at(1))); // pos i is 2nd
1122  data_list.emplace_back(static_cast<uint16_t>(raw_data.at(0).at(0))); // pos d is 1st
1123  data_list.emplace_back(0); // no vel p
1124  data_list.emplace_back(0); // no vel i
1125  data_list.emplace_back(0); // no ff1
1126  data_list.emplace_back(0); // no ff2
1127  }
1128  else
1129  {
1130  std::cout << "Failures during read PID gains: " << res << std::endl;
1131  return COMM_TX_FAIL;
1132  }
1133 
1134  return COMM_SUCCESS;
1135  }
1136 
1144  template <>
1145  inline int DxlDriver<XL320Reg>::writePID(uint8_t id, const std::vector<uint16_t> &data_list)
1146  {
1147  int res = COMM_TX_FAIL;
1148 
1149  // only rewrite params which is not success
1150  for (int tries = 10; tries > 0; --tries)
1151  {
1152  res = write<typename XL320Reg::TYPE_PID_GAIN>(XL320Reg::ADDR_POSITION_P_GAIN, id,
1153  static_cast<typename XL320Reg::TYPE_PID_GAIN>(data_list.at(0)));
1154  if (COMM_SUCCESS == res)
1155  break;
1156  }
1157  if (COMM_SUCCESS != res)
1158  return res;
1159 
1160  for (int tries = 10; tries > 0; --tries)
1161  {
1162  res = write<typename XL320Reg::TYPE_PID_GAIN>(XL320Reg::ADDR_POSITION_I_GAIN, id,
1163  static_cast<typename XL320Reg::TYPE_PID_GAIN>(data_list.at(1)));
1164  if (COMM_SUCCESS == res)
1165  break;
1166  }
1167  if (COMM_SUCCESS != res)
1168  return res;
1169 
1170  for (int tries = 10; tries > 0; --tries)
1171  {
1172  res = write<typename XL320Reg::TYPE_PID_GAIN>(XL320Reg::ADDR_POSITION_D_GAIN, id,
1173  static_cast<typename XL320Reg::TYPE_PID_GAIN>(data_list.at(2)));
1174  if (COMM_SUCCESS == res)
1175  break;
1176  }
1177 
1178  return res;
1179  }
1180 
1187  template <>
1188  inline int DxlDriver<XL320Reg>::syncWritePositionGoal(const std::vector<uint8_t> &id_list, const std::vector<uint32_t> &position_list)
1189  {
1190  std::vector<typename XL320Reg::TYPE_GOAL_POSITION> casted_list;
1191  casted_list.reserve(position_list.size());
1192  for (auto const &p : position_list)
1193  {
1194  casted_list.emplace_back(static_cast<typename XL320Reg::TYPE_GOAL_POSITION>(p));
1195  }
1196  return syncWrite<typename XL320Reg::TYPE_GOAL_POSITION>(XL320Reg::ADDR_GOAL_POSITION, id_list, casted_list);
1197  }
1198 
1205  template <>
1206  inline int DxlDriver<XL320Reg>::syncWriteVelocityGoal(const std::vector<uint8_t> &id_list, const std::vector<uint32_t> &velocity_list)
1207  {
1208  std::vector<typename XL320Reg::TYPE_GOAL_VELOCITY> casted_list;
1209  casted_list.reserve(velocity_list.size());
1210  for (auto const &v : velocity_list)
1211  {
1212  casted_list.emplace_back(static_cast<typename XL320Reg::TYPE_GOAL_VELOCITY>(v));
1213  }
1214  return syncWrite<typename XL320Reg::TYPE_GOAL_VELOCITY>(XL320Reg::ADDR_GOAL_VELOCITY, id_list, casted_list);
1215  }
1216 
1217  template <>
1218  inline int DxlDriver<XL320Reg>::readControlMode(uint8_t /*id*/, uint8_t & /*data*/)
1219  {
1220  std::cout << "readControlMode not available for motor XL320" << std::endl;
1221  return COMM_SUCCESS;
1222  }
1223 
1224  // XL430
1225 
1226  template <>
1227  inline std::string DxlDriver<XL430Reg>::interpretErrorState(uint32_t hw_state) const
1228  {
1229  std::string hardware_message;
1230 
1231  if (hw_state & 1 << 0) // 0b00000001
1232  {
1233  hardware_message += "Input Voltage";
1234  }
1235  if (hw_state & 1 << 2) // 0b00000100
1236  {
1237  if (!hardware_message.empty())
1238  hardware_message += ", ";
1239  hardware_message += "OverHeating";
1240  }
1241  if (hw_state & 1 << 3) // 0b00001000
1242  {
1243  if (!hardware_message.empty())
1244  hardware_message += ", ";
1245  hardware_message += "Motor Encoder";
1246  }
1247  if (hw_state & 1 << 4) // 0b00010000
1248  {
1249  if (!hardware_message.empty())
1250  hardware_message += ", ";
1251  hardware_message += "Electrical Shock";
1252  }
1253  if (hw_state & 1 << 5) // 0b00100000
1254  {
1255  if (!hardware_message.empty())
1256  hardware_message += ", ";
1257  hardware_message += "Overload";
1258  }
1259  if (hw_state & 1 << 7) // 0b10000000 => added by us : disconnected error
1260  {
1261  if (!hardware_message.empty())
1262  hardware_message += ", ";
1263  hardware_message += "Disconnection";
1264  }
1265  if (!hardware_message.empty())
1266  hardware_message += " Error";
1267 
1268  return hardware_message;
1269  }
1270 
1271  template <>
1272  inline int DxlDriver<XL430Reg>::writeTorqueGoal(uint8_t /*id*/, uint16_t /*torque*/)
1273  {
1274  std::cout << "writeTorqueGoal not available for motor XL430" << std::endl;
1275  return COMM_TX_ERROR;
1276  }
1277 
1278  template <>
1279  inline int DxlDriver<XL430Reg>::syncWriteTorqueGoal(const std::vector<uint8_t> & /*id_list*/, const std::vector<uint16_t> & /*torque_list*/)
1280  {
1281  std::cout << "syncWriteTorqueGoal not available for motor XL430" << std::endl;
1282  return COMM_TX_ERROR;
1283  }
1284 
1285  // XC430
1286 
1287  template <>
1288  inline std::string DxlDriver<XC430Reg>::interpretErrorState(uint32_t hw_state) const
1289  {
1290  std::string hardware_message;
1291 
1292  if (hw_state & 1 << 0) // 0b00000001
1293  {
1294  hardware_message += "Input Voltage";
1295  }
1296  if (hw_state & 1 << 2) // 0b00000100
1297  {
1298  if (!hardware_message.empty())
1299  hardware_message += ", ";
1300  hardware_message += "OverHeating";
1301  }
1302  if (hw_state & 1 << 3) // 0b00001000
1303  {
1304  if (!hardware_message.empty())
1305  hardware_message += ", ";
1306  hardware_message += "Motor Encoder";
1307  }
1308  if (hw_state & 1 << 4) // 0b00010000
1309  {
1310  if (!hardware_message.empty())
1311  hardware_message += ", ";
1312  hardware_message += "Electrical Shock";
1313  }
1314  if (hw_state & 1 << 5) // 0b00100000
1315  {
1316  if (!hardware_message.empty())
1317  hardware_message += ", ";
1318  hardware_message += "Overload";
1319  }
1320  if (hw_state & 1 << 7) // 0b10000000 => added by us : disconnected error
1321  {
1322  if (!hardware_message.empty())
1323  hardware_message += ", ";
1324  hardware_message += "Disconnection";
1325  }
1326  if (!hardware_message.empty())
1327  hardware_message += " Error";
1328 
1329  return hardware_message;
1330  }
1331 
1332  template <>
1333  inline int DxlDriver<XC430Reg>::writeTorqueGoal(uint8_t /*id*/, uint16_t /*torque*/)
1334  {
1335  std::cout << "writeTorqueGoal not available for motor XC430" << std::endl;
1336  return COMM_TX_ERROR;
1337  }
1338 
1339  template <>
1340  inline int DxlDriver<XC430Reg>::syncWriteTorqueGoal(const std::vector<uint8_t> & /*id_list*/, const std::vector<uint16_t> & /*torque_list*/)
1341  {
1342  std::cout << "syncWriteTorqueGoal not available for motor XC430" << std::endl;
1343  return COMM_TX_ERROR;
1344  }
1345 
1346  // XM430
1347 
1348  template <>
1349  inline std::string DxlDriver<XM430Reg>::interpretErrorState(uint32_t hw_state) const
1350  {
1351  std::string hardware_message;
1352 
1353  if (hw_state & 1 << 0) // 0b00000001
1354  {
1355  hardware_message += "Input Voltage";
1356  }
1357  if (hw_state & 1 << 2) // 0b00000100
1358  {
1359  if (!hardware_message.empty())
1360  hardware_message += ", ";
1361  hardware_message += "OverHeating";
1362  }
1363  if (hw_state & 1 << 3) // 0b00001000
1364  {
1365  if (!hardware_message.empty())
1366  hardware_message += ", ";
1367  hardware_message += "Motor Encoder";
1368  }
1369  if (hw_state & 1 << 4) // 0b00010000
1370  {
1371  if (!hardware_message.empty())
1372  hardware_message += ", ";
1373  hardware_message += "Electrical Shock";
1374  }
1375  if (hw_state & 1 << 5) // 0b00100000
1376  {
1377  if (!hardware_message.empty())
1378  hardware_message += ", ";
1379  hardware_message += "Overload";
1380  }
1381  if (hw_state & 1 << 7) // 0b10000000 => added by us : disconnected error
1382  {
1383  if (!hardware_message.empty())
1384  hardware_message += ", ";
1385  hardware_message += "Disconnection";
1386  }
1387  if (!hardware_message.empty())
1388  hardware_message += " Error";
1389 
1390  return hardware_message;
1391  }
1392 
1393  // works with current instead of load
1394 
1395  template <>
1396  inline int DxlDriver<XM430Reg>::writeTorqueGoal(uint8_t id, uint16_t torque)
1397  {
1398  return write<typename XM430Reg::TYPE_GOAL_CURRENT>(XM430Reg::ADDR_GOAL_CURRENT, id, torque);
1399  }
1400 
1401  template <>
1402  inline int DxlDriver<XM430Reg>::syncWriteTorqueGoal(const std::vector<uint8_t> &id_list, const std::vector<uint16_t> &torque_list)
1403  {
1404  return syncWrite<typename XM430Reg::TYPE_GOAL_CURRENT>(XM430Reg::ADDR_GOAL_CURRENT, id_list, torque_list);
1405  }
1406 
1407  template <>
1408  inline int DxlDriver<XM430Reg>::readLoad(uint8_t id, uint16_t &present_load)
1409  {
1410  return read<typename XM430Reg::TYPE_PRESENT_CURRENT>(XM430Reg::ADDR_PRESENT_CURRENT, id, present_load);
1411  }
1412 
1413  template <>
1414  inline int DxlDriver<XM430Reg>::syncReadLoad(const std::vector<uint8_t> &id_list, std::vector<uint16_t> &load_list)
1415  {
1416  return syncRead<typename XM430Reg::TYPE_PRESENT_CURRENT>(XM430Reg::ADDR_PRESENT_CURRENT, id_list, load_list);
1417  }
1418 
1419  // XL330
1420 
1421  template <>
1422  inline std::string DxlDriver<XL330Reg>::interpretErrorState(uint32_t hw_state) const
1423  {
1424  std::string hardware_message;
1425 
1426  if (hw_state & 1 << 0) // 0b00000001
1427  {
1428  hardware_message += "Input Voltage";
1429  }
1430  if (hw_state & 1 << 2) // 0b00000100
1431  {
1432  if (!hardware_message.empty())
1433  hardware_message += ", ";
1434  hardware_message += "OverHeating";
1435  }
1436  if (hw_state & 1 << 3) // 0b00001000
1437  {
1438  if (!hardware_message.empty())
1439  hardware_message += ", ";
1440  hardware_message += "Motor Encoder";
1441  }
1442  if (hw_state & 1 << 4) // 0b00010000
1443  {
1444  if (!hardware_message.empty())
1445  hardware_message += ", ";
1446  hardware_message += "Electrical Shock";
1447  }
1448  if (hw_state & 1 << 5) // 0b00100000
1449  {
1450  if (!hardware_message.empty())
1451  hardware_message += ", ";
1452  hardware_message += "Overload";
1453  }
1454  if (hw_state & 1 << 7) // 0b10000000 => added by us : disconnected error
1455  {
1456  if (!hardware_message.empty())
1457  hardware_message += ", ";
1458  hardware_message += "Disconnection";
1459  }
1460  if (!hardware_message.empty())
1461  hardware_message += " Error";
1462 
1463  return hardware_message;
1464  }
1465 
1466  // works with current instead of load
1467 
1468  template <>
1469  inline int DxlDriver<XL330Reg>::writeTorqueGoal(uint8_t id, uint16_t torque)
1470  {
1471  return write<typename XL330Reg::TYPE_GOAL_CURRENT>(XL330Reg::ADDR_GOAL_CURRENT, id, torque);
1472  }
1473 
1474  template <>
1475  inline int DxlDriver<XL330Reg>::syncWriteTorqueGoal(const std::vector<uint8_t> &id_list, const std::vector<uint16_t> &torque_list)
1476  {
1477  return syncWrite<typename XL330Reg::TYPE_GOAL_CURRENT>(XL330Reg::ADDR_GOAL_CURRENT, id_list, torque_list);
1478  }
1479 
1480  template <>
1481  inline int DxlDriver<XL330Reg>::readLoad(uint8_t id, uint16_t &present_load)
1482  {
1483  return read<typename XL330Reg::TYPE_PRESENT_CURRENT>(XL330Reg::ADDR_PRESENT_CURRENT, id, present_load);
1484  }
1485 
1486  template <>
1487  inline int DxlDriver<XL330Reg>::syncReadLoad(const std::vector<uint8_t> &id_list, std::vector<uint16_t> &load_list)
1488  {
1489  return syncRead<typename XL330Reg::TYPE_PRESENT_CURRENT>(XL330Reg::ADDR_PRESENT_CURRENT, id_list, load_list);
1490  }
1491 
1492  template <>
1493  inline int DxlDriver<XL330Reg>::writeTemperatureLimit(uint8_t id, uint8_t temperature_limit)
1494  {
1495  return write<typename XL330Reg::TYPE_TEMPERATURE_LIMIT>(XL330Reg::ADDR_TEMPERATURE_LIMIT, id, temperature_limit);
1496  }
1497 
1498  template <>
1499  inline int DxlDriver<XL330Reg>::writeShutdownConfiguration(uint8_t id, uint8_t configuration)
1500  {
1501  return write<typename XL330Reg::TYPE_ALARM_SHUTDOWN>(XL330Reg::ADDR_ALARM_SHUTDOWN, id, configuration);
1502  }
1503 
1504  // XH430
1505 
1506  template <>
1507  inline std::string DxlDriver<XH430Reg>::interpretErrorState(uint32_t hw_state) const
1508  {
1509  std::string hardware_message;
1510 
1511  if (hw_state & 1 << 0) // 0b00000001
1512  {
1513  hardware_message += "Input Voltage";
1514  }
1515  if (hw_state & 1 << 2) // 0b00000100
1516  {
1517  if (!hardware_message.empty())
1518  hardware_message += ", ";
1519  hardware_message += "OverHeating";
1520  }
1521  if (hw_state & 1 << 3) // 0b00001000
1522  {
1523  if (!hardware_message.empty())
1524  hardware_message += ", ";
1525  hardware_message += "Motor Encoder";
1526  }
1527  if (hw_state & 1 << 4) // 0b00010000
1528  {
1529  if (!hardware_message.empty())
1530  hardware_message += ", ";
1531  hardware_message += "Electrical Shock";
1532  }
1533  if (hw_state & 1 << 5) // 0b00100000
1534  {
1535  if (!hardware_message.empty())
1536  hardware_message += ", ";
1537  hardware_message += "Overload";
1538  }
1539  if (hw_state & 1 << 7) // 0b10000000 => added by us : disconnected error
1540  {
1541  if (!hardware_message.empty())
1542  hardware_message += ", ";
1543  hardware_message += "Disconnection";
1544  }
1545  if (!hardware_message.empty())
1546  hardware_message += " Error";
1547 
1548  return hardware_message;
1549  }
1550 
1551  template <>
1552  inline int DxlDriver<XH430Reg>::writeTorqueGoal(uint8_t id, uint16_t torque)
1553  {
1554  return write<typename XL330Reg::TYPE_GOAL_CURRENT>(XL330Reg::ADDR_GOAL_CURRENT, id, torque);
1555  }
1556 
1557  template <>
1558  inline int DxlDriver<XH430Reg>::syncWriteTorqueGoal(const std::vector<uint8_t> & id_list, const std::vector<uint16_t> & torque_list)
1559  {
1560  return syncWrite<typename XL330Reg::TYPE_GOAL_CURRENT>(XL330Reg::ADDR_GOAL_CURRENT, id_list, torque_list);
1561  }
1562 
1563 } // ttl_driver
1564 
1565 #endif // DxlDriver
ttl_driver::DxlDriver::writePositionGoal
int writePositionGoal(uint8_t id, uint32_t position) override
DxlDriver<reg_type>::writePositionGoal.
Definition: dxl_driver.hpp:366
ttl_driver::DxlDriver::syncReadHwErrorStatus
int syncReadHwErrorStatus(const std::vector< uint8_t > &id_list, std::vector< uint8_t > &hw_error_list) override
DxlDriver<reg_type>::syncReadHwErrorStatus.
Definition: dxl_driver.hpp:684
ttl_driver::XL320Reg::TYPE_PRESENT_VELOCITY
uint16_t TYPE_PRESENT_VELOCITY
Definition: xl320_reg.hpp:102
ttl_driver::XL320Reg::TYPE_PRESENT_POSITION
uint16_t TYPE_PRESENT_POSITION
Definition: xl320_reg.hpp:99
ttl_driver::DxlDriver::syncReadHwStatus
int syncReadHwStatus(const std::vector< uint8_t > &id_list, std::vector< std::pair< double, uint8_t >> &data_list) override
DxlDriver<reg_type>::syncReadHwStatus.
Definition: dxl_driver.hpp:655
ttl_driver::XL320Reg::ADDR_PRESENT_VELOCITY
static constexpr uint16_t ADDR_PRESENT_VELOCITY
Definition: xl320_reg.hpp:101
ttl_driver::DxlDriver::syncReadLoad
int syncReadLoad(const std::vector< uint8_t > &id_list, std::vector< uint16_t > &load_list) override
DxlDriver<reg_type>::syncReadLoad.
Definition: dxl_driver.hpp:855
ttl_driver::XL320Reg::ADDR_POSITION_P_GAIN
static constexpr uint16_t ADDR_POSITION_P_GAIN
Definition: xl320_reg.hpp:87
ttl_driver::XL320Reg::ADDR_POSITION_I_GAIN
static constexpr uint16_t ADDR_POSITION_I_GAIN
Definition: xl320_reg.hpp:86
ttl_driver::DxlDriver::readTemperature
int readTemperature(uint8_t id, uint8_t &temperature) override
DxlDriver<reg_type>::readTemperature.
Definition: dxl_driver.hpp:464
ttl_driver::XL330Reg::ADDR_TEMPERATURE_LIMIT
static constexpr uint16_t ADDR_TEMPERATURE_LIMIT
Definition: xl330_reg.hpp:62
ttl_driver::DxlDriver::readVelocityProfile
int readVelocityProfile(uint8_t id, std::vector< uint32_t > &data_list) override
Definition: dxl_driver.hpp:429
ttl_driver::DxlDriver::writeTorquePercentage
int writeTorquePercentage(uint8_t id, uint8_t torque_percentage) override
DxlDriver<reg_type>::writeTorquePercentage.
Definition: dxl_driver.hpp:353
ttl_driver::DxlDriver::syncWriteTorqueGoal
int syncWriteTorqueGoal(const std::vector< uint8_t > &id_list, const std::vector< uint16_t > &torque_list) override
DxlDriver<reg_type>::syncWriteTorqueGoal.
Definition: dxl_driver.hpp:736
ttl_driver::DxlDriver::writePID
int writePID(uint8_t id, const std::vector< uint16_t > &data) override
DxlDriver<reg_type>::writePID.
Definition: dxl_driver.hpp:515
ttl_driver::DxlDriver::writeTemperatureLimit
int writeTemperatureLimit(uint8_t id, uint8_t temperature_limit) override
DxlDriver<reg_type>::writeTemperatureLimit.
Definition: dxl_driver.hpp:268
xl320_reg.hpp
ttl_driver::AbstractDxlDriver
Definition: abstract_dxl_driver.hpp:33
ttl_driver::DxlDriver::syncWriteLed
int syncWriteLed(const std::vector< uint8_t > &id_list, const std::vector< uint8_t > &led_list) override
DxlDriver<reg_type>::syncWriteLed.
Definition: dxl_driver.hpp:712
ttl_driver::DxlDriver::writeTorqueGoal
int writeTorqueGoal(uint8_t id, uint16_t torque) override
DxlDriver<reg_type>::writeTorqueGoal.
Definition: dxl_driver.hpp:724
ttl_driver::DxlDriver::readHwErrorStatus
int readHwErrorStatus(uint8_t id, uint8_t &hardware_error_status) override
DxlDriver<reg_type>::readHwErrorStatus.
Definition: dxl_driver.hpp:491
ttl_driver::DxlDriver::str
std::string str() const override
DxlDriver<reg_type>::str.
Definition: dxl_driver.hpp:158
ttl_driver::DxlDriver::syncReadRawVoltage
int syncReadRawVoltage(const std::vector< uint8_t > &id_list, std::vector< double > &voltage_list) override
DxlDriver<reg_type>::syncReadRawVoltage.
Definition: dxl_driver.hpp:638
ttl_driver::DxlDriver::syncReadVelocity
int syncReadVelocity(const std::vector< uint8_t > &id_list, std::vector< uint32_t > &velocity_list) override
DxlDriver<reg_type>::syncReadVelocity.
Definition: dxl_driver.hpp:879
ttl_driver::XL330Reg::ADDR_GOAL_CURRENT
static constexpr uint16_t ADDR_GOAL_CURRENT
Definition: xl330_reg.hpp:124
ttl_driver::XM430Reg::ADDR_GOAL_CURRENT
static constexpr uint16_t ADDR_GOAL_CURRENT
Definition: xm430_reg.hpp:124
ttl_driver::DxlDriver::syncReadJointStatus
int syncReadJointStatus(const std::vector< uint8_t > &id_list, std::vector< std::array< uint32_t, 2 >> &data_array_list) override
DxlDriver::syncReadJointStatus.
Definition: dxl_driver.hpp:891
ttl_driver::DxlDriver::syncReadFirmwareVersion
int syncReadFirmwareVersion(const std::vector< uint8_t > &id_list, std::vector< std::string > &firmware_list) override
DxlDriver<reg_type>::syncReadFirmwareVersion.
Definition: dxl_driver.hpp:592
xc430_reg.hpp
ttl_driver::XL320Reg::ADDR_PRESENT_VOLTAGE
static constexpr uint16_t ADDR_PRESENT_VOLTAGE
Definition: xl320_reg.hpp:107
abstract_dxl_driver.hpp
ttl_driver::DxlDriver::readFirmwareVersion
int readFirmwareVersion(uint8_t id, std::string &version) override
DxlDriver<reg_type>::readFirmwareVersion.
Definition: dxl_driver.hpp:226
ttl_driver::XL320Reg::ADDR_GOAL_POSITION
static constexpr uint16_t ADDR_GOAL_POSITION
Definition: xl320_reg.hpp:89
ttl_driver::DxlDriver::writeShutdownConfiguration
int writeShutdownConfiguration(uint8_t id, uint8_t configuration) override
DxlDriver<reg_type>::writeShutdownConfiguration.
Definition: dxl_driver.hpp:281
ttl_driver::XL320Reg::TYPE_GOAL_POSITION
uint16_t TYPE_GOAL_POSITION
Definition: xl320_reg.hpp:90
xl430_reg.hpp
ttl_driver::DxlDriver
The DxlDriver class.
Definition: dxl_driver.hpp:44
ttl_driver::DxlDriver::readVoltage
int readVoltage(uint8_t id, double &voltage) override
DxlDriver<reg_type>::readVoltage.
Definition: dxl_driver.hpp:476
ttl_driver::XL320Reg::TYPE_GOAL_VELOCITY
uint16_t TYPE_GOAL_VELOCITY
Definition: xl320_reg.hpp:93
ttl_driver::DxlDriver::syncWriteTorquePercentage
int syncWriteTorquePercentage(const std::vector< uint8_t > &id_list, const std::vector< uint8_t > &torque_percentage_list) override
DxlDriver<reg_type>::syncWriteTorquePercentage.
Definition: dxl_driver.hpp:392
ttl_driver::DxlDriver::readControlMode
int readControlMode(uint8_t id, uint8_t &control_mode) override
DxlDriver<reg_type>::readControlMode.
Definition: dxl_driver.hpp:826
ttl_driver::DxlDriver::changeId
int changeId(uint8_t id, uint8_t new_id) override
DxlDriver<reg_type>::changeId.
Definition: dxl_driver.hpp:193
ttl_driver
Definition: abstract_dxl_driver.hpp:30
ttl_driver::DxlDriver::writeControlMode
int writeControlMode(uint8_t id, uint8_t control_mode) override
DxlDriver<reg_type>::writeControlMode.
Definition: dxl_driver.hpp:804
ttl_driver::XL320Reg::ADDR_GOAL_VELOCITY
static constexpr uint16_t ADDR_GOAL_VELOCITY
Definition: xl320_reg.hpp:92
ttl_driver::DxlDriver::readVelocity
int readVelocity(uint8_t id, uint32_t &present_velocity) override
DxlDriver<reg_type>::readVelocity.
Definition: dxl_driver.hpp:867
ttl_driver::XM430Reg::ADDR_PRESENT_CURRENT
static constexpr uint16_t ADDR_PRESENT_CURRENT
Definition: xm430_reg.hpp:146
ttl_driver::DxlDriver::syncReadVoltage
int syncReadVoltage(const std::vector< uint8_t > &id_list, std::vector< double > &voltage_list) override
DxlDriver<reg_type>::syncReadVoltage.
Definition: dxl_driver.hpp:621
ttl_driver::DxlDriver::DxlDriver
DxlDriver(std::shared_ptr< dynamixel::PortHandler > portHandler, std::shared_ptr< dynamixel::PacketHandler > packetHandler)
DxlDriver<reg_type>::DxlDriver.
Definition: dxl_driver.hpp:143
ttl_driver::DxlDriver::readPID
int readPID(uint8_t id, std::vector< uint16_t > &data_list) override
DxlDriver<reg_type>::readPID.
Definition: dxl_driver.hpp:750
ttl_driver::DxlDriver::syncWriteVelocityGoal
int syncWriteVelocityGoal(const std::vector< uint8_t > &id_list, const std::vector< uint32_t > &velocity_list) override
DxlDriver<reg_type>::syncWriteVelocityGoal.
Definition: dxl_driver.hpp:422
xh430_reg.hpp
ttl_driver::DxlDriver::syncWritePositionGoal
int syncWritePositionGoal(const std::vector< uint8_t > &id_list, const std::vector< uint32_t > &position_list) override
DxlDriver<reg_type>::syncWritePositionGoal.
Definition: dxl_driver.hpp:410
ttl_driver::DxlDriver::writeStartupConfiguration
int writeStartupConfiguration(uint8_t id, uint8_t config) override
DxlDriver<reg_type>::writeStartupConfiguration.
Definition: dxl_driver.hpp:242
ttl_driver::DxlDriver::readLoad
int readLoad(uint8_t id, uint16_t &present_load) override
DxlDriver<reg_type>::readLoad.
Definition: dxl_driver.hpp:843
xl330_reg.hpp
xm430_reg.hpp
ttl_driver::DxlDriver::readPosition
int readPosition(uint8_t id, uint32_t &present_position) override
DxlDriver<reg_type>::readPosition.
Definition: dxl_driver.hpp:452
ttl_driver::XL330Reg::ADDR_ALARM_SHUTDOWN
static constexpr uint16_t ADDR_ALARM_SHUTDOWN
Definition: xl330_reg.hpp:90
ttl_driver::XL320Reg::ADDR_POSITION_D_GAIN
static constexpr uint16_t ADDR_POSITION_D_GAIN
Definition: xl320_reg.hpp:85
ttl_driver::AbstractDxlDriver::str
std::string str() const override
AbstractTtlDriver::str : build a string describing the object. For debug purpose only.
Definition: abstract_dxl_driver.cpp:42
ttl_driver::DxlDriver::readMaxPosition
int readMaxPosition(uint8_t id, uint32_t &max_pos) override
DxlDriver<reg_type>::readMaxPosition.
Definition: dxl_driver.hpp:306
ttl_driver::DxlDriver::writeLed
int writeLed(uint8_t id, uint8_t led_value) override
DxlDriver<reg_type>::writeLed.
Definition: dxl_driver.hpp:700
ttl_driver::DxlDriver::syncReadTemperature
int syncReadTemperature(const std::vector< uint8_t > &id_list, std::vector< uint8_t > &temperature_list) override
DxlDriver<reg_type>::syncReadTemperature.
Definition: dxl_driver.hpp:609
ttl_driver::DxlDriver::interpretFirmwareVersion
std::string interpretFirmwareVersion(uint32_t fw_version) const override
DxlDriver<reg_type>::interpretFirmwareVersion.
Definition: dxl_driver.hpp:179
ttl_driver::DxlDriver::checkModelNumber
int checkModelNumber(uint8_t id, uint16_t &model_number) override
DxlDriver<reg_type>::checkModelNumber.
Definition: dxl_driver.hpp:204
ttl_driver::XL320Reg::TYPE_PID_GAIN
uint8_t TYPE_PID_GAIN
Definition: xl320_reg.hpp:84
ttl_driver::DxlDriver::readMinPosition
int readMinPosition(uint8_t id, uint32_t &min_pos) override
DxlDriver<reg_type>::readMinPosition.
Definition: dxl_driver.hpp:294
ttl_driver::DxlDriver::readMoving
int readMoving(uint8_t id, uint8_t &status) override
DxlDriver<reg_type>::readMoving.
Definition: dxl_driver.hpp:792
ttl_driver::DxlDriver::interpretErrorState
std::string interpretErrorState(uint32_t hw_state) const override
DxlDriver<reg_type>::interpretErrorState.
Definition: dxl_driver.hpp:168
ttl_driver::DxlDriver::writeVelocityProfile
int writeVelocityProfile(uint8_t id, const std::vector< uint32_t > &data_list) override
DxlDriver<reg_type>::writeVelocityProfile Write velocity profile for dxl.
Definition: dxl_driver.hpp:321
ttl_driver::DxlDriver::syncReadPosition
int syncReadPosition(const std::vector< uint8_t > &id_list, std::vector< uint32_t > &position_list) override
DxlDriver<reg_type>::syncReadPosition.
Definition: dxl_driver.hpp:503
ttl_driver::XL320Reg::ADDR_PRESENT_POSITION
static constexpr uint16_t ADDR_PRESENT_POSITION
Definition: xl320_reg.hpp:98
ttl_driver::DxlDriver::writeVelocityGoal
int writeVelocityGoal(uint8_t id, uint32_t velocity) override
DxlDriver<reg_type>::writeVelocityGoal.
Definition: dxl_driver.hpp:379
ttl_driver::XL330Reg::ADDR_PRESENT_CURRENT
static constexpr uint16_t ADDR_PRESENT_CURRENT
Definition: xl330_reg.hpp:146

ttl_driver
Author(s): Clement Cocquempot
autogenerated on Tue Jan 13 2026 12:43:08