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) 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)
205  {
206  uint16_t model_number = 0;
207  int ping_result = getModelNumber(id, model_number);
208 
209  if (ping_result == COMM_SUCCESS)
210  {
211  if (model_number && model_number != reg_type::MODEL_NUMBER)
212  {
213  return PING_WRONG_MODEL_NUMBER;
214  }
215  }
216 
217  return ping_result;
218  }
219 
226  template <typename reg_type>
227  int DxlDriver<reg_type>::readFirmwareVersion(uint8_t id, std::string &version)
228  {
229  int res = COMM_RX_FAIL;
230  uint8_t data{};
231  res = read<typename reg_type::TYPE_FIRMWARE_VERSION>(reg_type::ADDR_FIRMWARE_VERSION, id, data);
232  version = interpretFirmwareVersion(data);
233  return res;
234  }
235 
242  template <typename reg_type>
243  int DxlDriver<reg_type>::writeStartupConfiguration(uint8_t id, uint8_t config)
244  {
245  std::string version;
246  int res = readFirmwareVersion(id, version);
247 
248  // only for version above certain version (see registers)
249  if (COMM_SUCCESS == res && std::stoi(version) >= reg_type::VERSION_STARTUP_CONFIGURATION)
250  {
251  res = write<typename reg_type::TYPE_STARTUP_CONFIGURATION>(reg_type::ADDR_STARTUP_CONFIGURATION, id, config);
252  }
253  else
254  {
255  std::cout << "Startup configuration available only for version > " << static_cast<int>(reg_type::VERSION_STARTUP_CONFIGURATION) << std::endl;
256  res = COMM_SUCCESS;
257  }
258 
259  return res;
260  }
261 
268  template <typename reg_type>
269  int DxlDriver<reg_type>::writeTemperatureLimit(uint8_t id, uint8_t temperature_limit)
270  {
271  std::cout << "temperature limit not available for this motor type" << std::endl;
272  return COMM_SUCCESS;
273  }
274 
281  template <typename reg_type>
282  int DxlDriver<reg_type>::writeShutdownConfiguration(uint8_t id, uint8_t configuration)
283  {
284  std::cout << "shutdown configuration not available for this motor type" << std::endl;
285  return COMM_SUCCESS;
286  }
287 
294  template <typename reg_type>
295  int DxlDriver<reg_type>::readMinPosition(uint8_t id, uint32_t &pos)
296  {
297  return read<typename reg_type::TYPE_MIN_POSITION_LIMIT>(reg_type::ADDR_MIN_POSITION_LIMIT, id, pos);
298  }
299 
306  template <typename reg_type>
307  int DxlDriver<reg_type>::readMaxPosition(uint8_t id, uint32_t &pos)
308  {
309  return read<typename reg_type::TYPE_MAX_POSITION_LIMIT>(reg_type::ADDR_MAX_POSITION_LIMIT, id, pos);
310  }
311 
312  // ram write
313 
321  template <typename reg_type>
322  int DxlDriver<reg_type>::writeVelocityProfile(uint8_t id, const std::vector<uint32_t> &data_list)
323  {
324  // in mode control Position Control Mode, velocity profile in datasheet is used to write velocity (except xl320)
325  int res = COMM_RX_FAIL;
326 
327  for (int tries = 10; tries > 0; --tries)
328  {
329  res = write<typename reg_type::TYPE_PROFILE>(reg_type::ADDR_PROFILE_VELOCITY, id, data_list.at(0));
330  if (COMM_SUCCESS == res)
331  break;
332  }
333 
334  for (int tries = 10; tries > 0; --tries)
335  {
336  res = write<typename reg_type::TYPE_PROFILE>(reg_type::ADDR_PROFILE_ACCELERATION, id, data_list.at(1));
337  if (COMM_SUCCESS == res)
338  break;
339  }
340 
341  if (COMM_SUCCESS != res)
342  return res;
343 
344  return res;
345  }
346 
353  template <typename reg_type>
354  int DxlDriver<reg_type>::writeTorquePercentage(uint8_t id, uint8_t torque_percentage)
355  {
356  auto torque_enable = torque_percentage > 0 ? 1 : 0;
357  return write<typename reg_type::TYPE_TORQUE_ENABLE>(reg_type::ADDR_TORQUE_ENABLE, id, torque_enable);
358  }
359 
366  template <typename reg_type>
367  int DxlDriver<reg_type>::writePositionGoal(uint8_t id, uint32_t position)
368  {
369  return write<typename reg_type::TYPE_GOAL_POSITION>(reg_type::ADDR_GOAL_POSITION, id,
370  static_cast<typename reg_type::TYPE_GOAL_POSITION>(position));
371  }
372 
379  template <typename reg_type>
380  int DxlDriver<reg_type>::writeVelocityGoal(uint8_t id, uint32_t velocity)
381  {
382  return write<typename reg_type::TYPE_GOAL_VELOCITY>(reg_type::ADDR_GOAL_VELOCITY, id,
383  static_cast<typename reg_type::TYPE_GOAL_VELOCITY>(velocity));
384  }
385 
392  template <typename reg_type>
393  int DxlDriver<reg_type>::syncWriteTorquePercentage(const std::vector<uint8_t> &id_list, const std::vector<uint8_t> &torque_percentage_list)
394  {
395  std::vector<uint8_t> torque_enable_list;
396  for(const auto &torque_percentage : torque_percentage_list)
397  {
398  auto torque_enable = torque_percentage > 0 ? 1 : 0;
399  torque_enable_list.push_back(torque_enable);
400  }
401  return syncWrite<typename reg_type::TYPE_TORQUE_ENABLE>(reg_type::ADDR_TORQUE_ENABLE, id_list, torque_enable_list);
402  }
403 
410  template <typename reg_type>
411  int DxlDriver<reg_type>::syncWritePositionGoal(const std::vector<uint8_t> &id_list, const std::vector<uint32_t> &position_list)
412  {
413  return syncWrite<typename reg_type::TYPE_GOAL_POSITION>(reg_type::ADDR_GOAL_POSITION, id_list, position_list);
414  }
415 
422  template <typename reg_type>
423  int DxlDriver<reg_type>::syncWriteVelocityGoal(const std::vector<uint8_t> &id_list, const std::vector<uint32_t> &velocity_list)
424  {
425  return syncWrite<typename reg_type::TYPE_GOAL_VELOCITY>(reg_type::ADDR_GOAL_VELOCITY, id_list, velocity_list);
426  }
427 
428  // ram read
429  template <typename reg_type>
430  int DxlDriver<reg_type>::readVelocityProfile(uint8_t id, std::vector<uint32_t> &data_list)
431  {
432  data_list.clear();
433  typename reg_type::TYPE_PROFILE velocity_profile{};
434  typename reg_type::TYPE_PROFILE acceleration_profile{};
435 
436  int res = read<typename reg_type::TYPE_PROFILE>(reg_type::ADDR_PROFILE_VELOCITY, id, velocity_profile);
437  if (COMM_SUCCESS == res)
438  res = read<typename reg_type::TYPE_PROFILE>(reg_type::ADDR_PROFILE_ACCELERATION, id, acceleration_profile);
439 
440  data_list.emplace_back(velocity_profile);
441  data_list.emplace_back(acceleration_profile);
442 
443  return res;
444  }
445 
452  template <typename reg_type>
453  int DxlDriver<reg_type>::readPosition(uint8_t id, uint32_t &present_position)
454  {
455  return read<typename reg_type::TYPE_PRESENT_POSITION>(reg_type::ADDR_PRESENT_POSITION, id, present_position);
456  }
457 
464  template <typename reg_type>
465  int DxlDriver<reg_type>::readTemperature(uint8_t id, uint8_t &temperature)
466  {
467  return read<typename reg_type::TYPE_PRESENT_TEMPERATURE>(reg_type::ADDR_PRESENT_TEMPERATURE, id, temperature);
468  }
469 
476  template <typename reg_type>
477  int DxlDriver<reg_type>::readVoltage(uint8_t id, double &voltage)
478  {
479  typename reg_type::TYPE_PRESENT_VOLTAGE voltage_mV{};
480  int res = read<typename reg_type::TYPE_PRESENT_VOLTAGE>(reg_type::ADDR_PRESENT_VOLTAGE, id, voltage_mV);
481  voltage = static_cast<double>(voltage_mV) / reg_type::VOLTAGE_CONVERSION;
482  return res;
483  }
484 
491  template <typename reg_type>
492  int DxlDriver<reg_type>::readHwErrorStatus(uint8_t id, uint8_t &hardware_error_status)
493  {
494  return read<typename reg_type::TYPE_HW_ERROR_STATUS>(reg_type::ADDR_HW_ERROR_STATUS, id, hardware_error_status);
495  }
496 
503  template <typename reg_type>
504  int DxlDriver<reg_type>::syncReadPosition(const std::vector<uint8_t> &id_list, std::vector<uint32_t> &position_list)
505  {
506  return syncRead<typename reg_type::TYPE_PRESENT_POSITION>(reg_type::ADDR_PRESENT_POSITION, id_list, position_list);
507  }
508 
515  template <typename reg_type>
516  int DxlDriver<reg_type>::writePID(uint8_t id, const std::vector<uint16_t> &data)
517  {
518  int res = COMM_TX_FAIL;
519 
520  // only rewrite params which is not success
521  for (int tries = 10; tries > 0; --tries)
522  {
523  res = write<typename reg_type::TYPE_PID_GAIN>(reg_type::ADDR_POSITION_P_GAIN, id, data.at(0));
524  if (COMM_SUCCESS == res)
525  break;
526  }
527 
528  if (COMM_SUCCESS != res)
529  return res;
530 
531  for (int tries = 10; tries > 0; --tries)
532  {
533  res = write<typename reg_type::TYPE_PID_GAIN>(reg_type::ADDR_POSITION_I_GAIN, id, data.at(1));
534  if (COMM_SUCCESS == res)
535  break;
536  }
537  if (COMM_SUCCESS != res)
538  return res;
539 
540  for (int tries = 10; tries > 0; --tries)
541  {
542  res = write<typename reg_type::TYPE_PID_GAIN>(reg_type::ADDR_POSITION_D_GAIN, id, data.at(2));
543  if (COMM_SUCCESS == res)
544  break;
545  }
546  if (COMM_SUCCESS != res)
547  return res;
548 
549  for (int tries = 10; tries > 0; --tries)
550  {
551  res = write<typename reg_type::TYPE_PID_GAIN>(reg_type::ADDR_VELOCITY_P_GAIN, id, data.at(3));
552  if (COMM_SUCCESS == res)
553  break;
554  }
555  if (COMM_SUCCESS != res)
556  return res;
557 
558  for (int tries = 10; tries > 0; --tries)
559  {
560  res = write<typename reg_type::TYPE_PID_GAIN>(reg_type::ADDR_VELOCITY_I_GAIN, id, data.at(4));
561  if (COMM_SUCCESS == res)
562  break;
563  }
564  if (COMM_SUCCESS != res)
565  return res;
566 
567  for (int tries = 10; tries > 0; --tries)
568  {
569  res = write<typename reg_type::TYPE_PID_GAIN>(reg_type::ADDR_FF1_GAIN, id, data.at(5));
570  if (COMM_SUCCESS == res)
571  break;
572  }
573  if (COMM_SUCCESS != res)
574  return res;
575 
576  for (int tries = 10; tries > 0; --tries)
577  {
578  res = write<typename reg_type::TYPE_PID_GAIN>(reg_type::ADDR_FF2_GAIN, id, data.at(6));
579  if (res == COMM_SUCCESS)
580  break;
581  }
582 
583  return res;
584  }
585 
592  template <typename reg_type>
593  int DxlDriver<reg_type>::syncReadFirmwareVersion(const std::vector<uint8_t> &id_list, std::vector<std::string> &firmware_list)
594  {
595  int res = COMM_RX_FAIL;
596  std::vector<uint8_t> data_list;
597  res = syncRead<typename reg_type::TYPE_FIRMWARE_VERSION>(reg_type::ADDR_FIRMWARE_VERSION, id_list, data_list);
598  for (auto const &data : data_list)
599  firmware_list.emplace_back(interpretFirmwareVersion(data));
600  return res;
601  }
602 
609  template <typename reg_type>
610  int DxlDriver<reg_type>::syncReadTemperature(const std::vector<uint8_t> &id_list, std::vector<uint8_t> &temperature_list)
611  {
612  return syncRead<typename reg_type::TYPE_PRESENT_TEMPERATURE>(reg_type::ADDR_PRESENT_TEMPERATURE, id_list, temperature_list);
613  }
614 
621  template <typename reg_type>
622  int DxlDriver<reg_type>::syncReadVoltage(const std::vector<uint8_t> &id_list, std::vector<double> &voltage_list)
623  {
624  voltage_list.clear();
625  std::vector<typename reg_type::TYPE_PRESENT_VOLTAGE> v_read;
626  int res = syncRead<typename reg_type::TYPE_PRESENT_VOLTAGE>(reg_type::ADDR_PRESENT_VOLTAGE, id_list, v_read);
627  for (auto const &v : v_read)
628  voltage_list.emplace_back(static_cast<double>(v) / reg_type::VOLTAGE_CONVERSION);
629  return res;
630  }
631 
638  template <typename reg_type>
639  int DxlDriver<reg_type>::syncReadRawVoltage(const std::vector<uint8_t> &id_list, std::vector<double> &voltage_list)
640  {
641  voltage_list.clear();
642  std::vector<typename reg_type::TYPE_PRESENT_VOLTAGE> v_read;
643  int res = syncRead<typename reg_type::TYPE_PRESENT_VOLTAGE>(reg_type::ADDR_PRESENT_VOLTAGE, id_list, v_read);
644  for (auto const &v : v_read)
645  voltage_list.emplace_back(static_cast<double>(v));
646  return res;
647  }
648 
655  template <typename reg_type>
656  int DxlDriver<reg_type>::syncReadHwStatus(const std::vector<uint8_t> &id_list,
657  std::vector<std::pair<double, uint8_t>> &data_list)
658  {
659  data_list.clear();
660 
661  std::vector<std::array<uint8_t, 3>> raw_data;
662  int res = syncReadConsecutiveBytes<uint8_t, 3>(reg_type::ADDR_PRESENT_VOLTAGE, id_list, raw_data);
663 
664  for (auto const &data : raw_data)
665  {
666  // Voltage is first reg, uint16
667  auto voltage = static_cast<double>((static_cast<uint16_t>(data.at(1)) << 8) | data.at(0));
668 
669  // Temperature is second reg, uint8
670  uint8_t temperature = data.at(2);
671 
672  data_list.emplace_back(std::make_pair(voltage, temperature));
673  }
674 
675  return res;
676  }
677 
684  template <typename reg_type>
685  int DxlDriver<reg_type>::syncReadHwErrorStatus(const std::vector<uint8_t> &id_list, std::vector<uint8_t> &hw_error_list)
686  {
687  return syncRead<typename reg_type::TYPE_HW_ERROR_STATUS>(reg_type::ADDR_HW_ERROR_STATUS, id_list, hw_error_list);
688  }
689 
690  //*****************************
691  // AbstractDxlDriver interface
692  //*****************************
693 
700  template <typename reg_type>
701  int DxlDriver<reg_type>::writeLed(uint8_t id, uint8_t led_value)
702  {
703  return write<typename reg_type::TYPE_LED>(reg_type::ADDR_LED, id, led_value);
704  }
705 
712  template <typename reg_type>
713  int DxlDriver<reg_type>::syncWriteLed(const std::vector<uint8_t> &id_list, const std::vector<uint8_t> &led_list)
714  {
715  return syncWrite<typename reg_type::TYPE_LED>(reg_type::ADDR_LED, id_list, led_list);
716  }
717 
724  template <typename reg_type>
725  int DxlDriver<reg_type>::writeTorqueGoal(uint8_t id, uint16_t torque)
726  {
727  return write<typename reg_type::TYPE_GOAL_TORQUE>(reg_type::ADDR_GOAL_TORQUE, id, torque);
728  }
729 
736  template <typename reg_type>
737  int DxlDriver<reg_type>::syncWriteTorqueGoal(const std::vector<uint8_t> &id_list, const std::vector<uint16_t> &torque_list)
738  {
739  return syncWrite<typename reg_type::TYPE_GOAL_TORQUE>(reg_type::ADDR_GOAL_TORQUE, id_list, torque_list);
740  }
741 
742  // read
743 
750  template <typename reg_type>
751  int DxlDriver<reg_type>::readPID(uint8_t id, std::vector<uint16_t> &data_list)
752  {
753  int res = 0;
754  data_list.clear();
755  std::vector<std::array<typename reg_type::TYPE_PID_GAIN, 8>> raw_data;
756 
757  // only rewrite params which is not success
758  for (int tries = 10; tries > 0; --tries)
759  {
760  res = syncReadConsecutiveBytes<typename reg_type::TYPE_PID_GAIN, 8>(reg_type::ADDR_VELOCITY_I_GAIN, {id}, raw_data);
761  if (COMM_SUCCESS == res)
762  break;
763  }
764 
765  // we need to reorder the data in its correct place in data_list
766  if (COMM_SUCCESS == res && raw_data.size() == 1)
767  {
768  data_list.emplace_back(raw_data.at(0).at(4)); // pos p is 5th
769  data_list.emplace_back(raw_data.at(0).at(3)); // pos i is 4th
770  data_list.emplace_back(raw_data.at(0).at(2)); // pos d is 3rd
771  data_list.emplace_back(raw_data.at(0).at(1)); // vel p is 2nd
772  data_list.emplace_back(raw_data.at(0).at(0)); // vel i is 1st
773  data_list.emplace_back(raw_data.at(0).at(7)); // ff1 is 8th
774  data_list.emplace_back(raw_data.at(0).at(6)); // ff2 is 7th
775  // nothing at 6th
776  }
777  else
778  {
779  std::cout << "Failures during read PID gains: " << res << std::endl;
780  return COMM_TX_FAIL;
781  }
782 
783  return COMM_SUCCESS;
784  }
785 
792  template <typename reg_type>
793  int DxlDriver<reg_type>::readMoving(uint8_t id, uint8_t &status)
794  {
795  return read<typename reg_type::TYPE_MOVING>(reg_type::ADDR_MOVING, id, status);
796  }
797 
804  template <typename reg_type>
805  int DxlDriver<reg_type>::writeControlMode(uint8_t id, uint8_t control_mode)
806  {
807  int res = COMM_TX_ERROR;
808  uint8_t torque{0};
809  res = read<typename reg_type::TYPE_TORQUE_ENABLE>(reg_type::ADDR_TORQUE_ENABLE, id, torque);
810  if (res == COMM_SUCCESS)
811  {
812  if (torque == 0)
813  {
814  return write<typename reg_type::TYPE_OPERATING_MODE>(reg_type::ADDR_OPERATING_MODE, id, control_mode);
815  }
816  }
817  return res;
818  }
819 
826  template <typename reg_type>
827  int DxlDriver<reg_type>::readControlMode(uint8_t id, uint8_t &control_mode)
828  {
829  int res = 0;
830  typename reg_type::TYPE_OPERATING_MODE raw{};
831  res = read<typename reg_type::TYPE_OPERATING_MODE>(reg_type::ADDR_OPERATING_MODE, id, raw);
832  control_mode = static_cast<uint8_t>(raw);
833  return res;
834  }
835  // other
836 
843  template <typename reg_type>
844  int DxlDriver<reg_type>::readLoad(uint8_t id, uint16_t &present_load)
845  {
846  return read<typename reg_type::TYPE_PRESENT_LOAD>(reg_type::ADDR_PRESENT_LOAD, id, present_load);
847  }
848 
855  template <typename reg_type>
856  int DxlDriver<reg_type>::syncReadLoad(const std::vector<uint8_t> &id_list, std::vector<uint16_t> &load_list)
857  {
858  return syncRead<typename reg_type::TYPE_PRESENT_LOAD>(reg_type::ADDR_PRESENT_LOAD, id_list, load_list);
859  }
860 
867  template <typename reg_type>
868  int DxlDriver<reg_type>::readVelocity(uint8_t id, uint32_t &present_velocity)
869  {
870  return read<typename reg_type::TYPE_PRESENT_VELOCITY>(reg_type::ADDR_PRESENT_VELOCITY, id, present_velocity);
871  }
872 
879  template <typename reg_type>
880  int DxlDriver<reg_type>::syncReadVelocity(const std::vector<uint8_t> &id_list, std::vector<uint32_t> &velocity_list)
881  {
882  return syncRead<typename reg_type::TYPE_PRESENT_VELOCITY>(reg_type::ADDR_PRESENT_VELOCITY, id_list, velocity_list);
883  }
884 
891  template <typename reg_type>
892  int DxlDriver<reg_type>::syncReadJointStatus(const std::vector<uint8_t> &id_list, std::vector<std::array<uint32_t, 2>> &data_array_list)
893  {
894  int res = COMM_TX_FAIL;
895 
896  if (id_list.empty())
897  return res;
898 
899  data_array_list.clear();
900 
901  // read torque enable on first id
902  typename reg_type::TYPE_TORQUE_ENABLE torque{1};
903  res = read<typename reg_type::TYPE_TORQUE_ENABLE>(reg_type::ADDR_TORQUE_ENABLE, id_list.at(0), torque);
904  if (COMM_SUCCESS != res)
905  std::cout << "#############"
906  " ERROR reading dxl torque in syncReadJointStatus (error "
907  << res << ")" << std::endl;
908 
909  // if torque on, read position and velocity
910  if (torque)
911  {
912  res = syncReadConsecutiveBytes<uint32_t, 2>(reg_type::ADDR_PRESENT_VELOCITY, id_list, data_array_list);
913  }
914  else // else read position only
915  {
916  std::vector<uint32_t> position_list;
917  res = syncReadPosition(id_list, position_list);
918  for (auto p : position_list)
919  data_array_list.emplace_back(std::array<uint32_t, 2>{0, p});
920  }
921 
922  return res;
923  }
924 
925  /*
926  * ----------------- specializations --------------------
927  */
928 
929  // XL320
930 
931  template <>
932  inline int DxlDriver<XL320Reg>::writeStartupConfiguration(uint8_t /*id*/, uint8_t /*config*/)
933  {
934  std::cout << "startup configuration for XL320 not available" << std::endl;
935  return COMM_SUCCESS;
936  }
937 
938  template <>
939  inline int DxlDriver<XL320Reg>::readMinPosition(uint8_t /*id*/, uint32_t &pos)
940  {
941  pos = 0;
942  std::cout << "min position hardcoded for motor XL320" << std::endl;
943  return COMM_SUCCESS;
944  }
945 
946  template <>
947  inline int DxlDriver<XL320Reg>::readMaxPosition(uint8_t /*id*/, uint32_t &pos)
948  {
949  pos = 1023;
950  std::cout << "max position hardcoded for motor XL320" << std::endl;
951  return COMM_SUCCESS;
952  }
953 
954  template <>
955  inline std::string DxlDriver<XL320Reg>::interpretErrorState(uint32_t hw_state) const
956  {
957  std::string hardware_message;
958 
959  if (hw_state & 1 << 0) // 0b00000001
960  {
961  hardware_message += "Overload";
962  }
963  if (hw_state & 1 << 1) // 0b00000010
964  {
965  if (!hardware_message.empty())
966  hardware_message += ", ";
967  hardware_message += "OverHeating";
968  }
969  if (hw_state & 1 << 2) // 0b00000100
970  {
971  if (!hardware_message.empty())
972  hardware_message += ", ";
973  hardware_message += "Input voltage out of range";
974  }
975  if (hw_state & 1 << 7) // 0b10000000 => added by us : disconnected error
976  {
977  if (!hardware_message.empty())
978  hardware_message += ", ";
979  hardware_message += "Disconnection";
980  }
981 
982  return hardware_message;
983  }
984 
985  template <>
986  inline int DxlDriver<XL320Reg>::readVelocityProfile(uint8_t /*id*/, std::vector<uint32_t> & /*data_list*/)
987  {
988  std::cout << "readVelocityProfile not available for XL320" << std::endl;
989  return COMM_SUCCESS;
990  }
991 
992  template <>
993  inline int DxlDriver<XL320Reg>::writeVelocityProfile(uint8_t /*id*/, const std::vector<uint32_t> & /*data_list*/)
994  {
995  std::cout << "writeVelocityProfile not available for XL320" << std::endl;
996  return COMM_SUCCESS;
997  }
998 
999  template <>
1000  inline int DxlDriver<XL320Reg>::readPosition(uint8_t id, uint32_t &present_position)
1001  {
1002  typename XL320Reg::TYPE_PRESENT_POSITION raw_data{};
1003  int res = read<typename XL320Reg::TYPE_PRESENT_POSITION>(XL320Reg::ADDR_PRESENT_POSITION, id, raw_data);
1004  present_position = raw_data;
1005  return res;
1006  }
1007 
1008  template <>
1009  inline int DxlDriver<XL320Reg>::syncReadPosition(const std::vector<uint8_t> &id_list, std::vector<uint32_t> &position_list)
1010  {
1011  position_list.clear();
1012  std::vector<typename XL320Reg::TYPE_PRESENT_POSITION> raw_data_list{};
1013 
1014  int res = syncRead<typename XL320Reg::TYPE_PRESENT_POSITION>(XL320Reg::ADDR_PRESENT_POSITION, id_list, raw_data_list);
1015  for (auto p : raw_data_list)
1016  position_list.emplace_back(p);
1017 
1018  return res;
1019  }
1020 
1021  template <>
1022  inline int DxlDriver<XL320Reg>::readVelocity(uint8_t id, uint32_t &present_velocity)
1023  {
1024  typename XL320Reg::TYPE_PRESENT_VELOCITY raw_data{};
1025  int res = read<typename XL320Reg::TYPE_PRESENT_VELOCITY>(XL320Reg::ADDR_PRESENT_VELOCITY, id, raw_data);
1026  present_velocity = raw_data;
1027 
1028  return res;
1029  }
1030 
1031  template <>
1032  inline int DxlDriver<XL320Reg>::syncReadVelocity(const std::vector<uint8_t> &id_list, std::vector<uint32_t> &velocity_list)
1033  {
1034  std::vector<typename XL320Reg::TYPE_PRESENT_VELOCITY> raw_data_list;
1035  int res = syncRead<typename XL320Reg::TYPE_PRESENT_VELOCITY>(XL320Reg::ADDR_PRESENT_VELOCITY, id_list, raw_data_list);
1036  for (auto v : raw_data_list)
1037  velocity_list.emplace_back(v);
1038  return res;
1039  }
1040 
1047  template <>
1048  inline int DxlDriver<XL320Reg>::syncReadJointStatus(const std::vector<uint8_t> &id_list, std::vector<std::array<uint32_t, 2>> &data_array_list)
1049  {
1050  int res = COMM_TX_FAIL;
1051 
1052  if (id_list.empty())
1053  return res;
1054 
1055  data_array_list.clear();
1056 
1057  std::vector<uint32_t> position_list;
1058  res = syncReadPosition(id_list, position_list);
1059  if (res == COMM_SUCCESS)
1060  {
1061  for (auto p : position_list)
1062  data_array_list.emplace_back(std::array<uint32_t, 2>{0, p});
1063  }
1064 
1065  return res;
1066  }
1067 
1074  template <>
1075  inline int DxlDriver<XL320Reg>::syncReadHwStatus(const std::vector<uint8_t> &id_list,
1076  std::vector<std::pair<double, uint8_t>> &data_list)
1077  {
1078  data_list.clear();
1079 
1080  std::vector<std::array<uint8_t, 2>> raw_data;
1081  int res = syncReadConsecutiveBytes<uint8_t, 2>(XL320Reg::ADDR_PRESENT_VOLTAGE, id_list, raw_data);
1082 
1083  for (auto const &data : raw_data)
1084  {
1085  // Voltage is first reg, uint16
1086  auto voltage = static_cast<double>(data.at(0));
1087 
1088  // Temperature is second reg, uint8
1089  uint8_t temperature = data.at(1);
1090 
1091  data_list.emplace_back(std::make_pair(voltage, temperature));
1092  }
1093 
1094  return res;
1095  }
1096 
1103  template <>
1104  inline int DxlDriver<XL320Reg>::readPID(uint8_t id, std::vector<uint16_t> &data_list)
1105  {
1106  int res = COMM_RX_FAIL;
1107  data_list.clear();
1108  std::vector<std::array<typename XL320Reg::TYPE_PID_GAIN, 3>> raw_data;
1109 
1110  // only rewrite params which is not success
1111  for (int tries = 10; tries > 0; --tries)
1112  {
1113  res = syncReadConsecutiveBytes<typename XL320Reg::TYPE_PID_GAIN, 3>(XL320Reg::ADDR_POSITION_D_GAIN, {id}, raw_data);
1114  if (COMM_SUCCESS == res)
1115  break;
1116  }
1117 
1118  // we need to reorder the data in its correct place in data_list
1119  if (COMM_SUCCESS == res && raw_data.size() == 1)
1120  {
1121  data_list.emplace_back(static_cast<uint16_t>(raw_data.at(0).at(2))); // pos p is 3rd
1122  data_list.emplace_back(static_cast<uint16_t>(raw_data.at(0).at(1))); // pos i is 2nd
1123  data_list.emplace_back(static_cast<uint16_t>(raw_data.at(0).at(0))); // pos d is 1st
1124  data_list.emplace_back(0); // no vel p
1125  data_list.emplace_back(0); // no vel i
1126  data_list.emplace_back(0); // no ff1
1127  data_list.emplace_back(0); // no ff2
1128  }
1129  else
1130  {
1131  std::cout << "Failures during read PID gains: " << res << std::endl;
1132  return COMM_TX_FAIL;
1133  }
1134 
1135  return COMM_SUCCESS;
1136  }
1137 
1145  template <>
1146  inline int DxlDriver<XL320Reg>::writePID(uint8_t id, const std::vector<uint16_t> &data_list)
1147  {
1148  int res = COMM_TX_FAIL;
1149 
1150  // only rewrite params which is not success
1151  for (int tries = 10; tries > 0; --tries)
1152  {
1153  res = write<typename XL320Reg::TYPE_PID_GAIN>(XL320Reg::ADDR_POSITION_P_GAIN, id,
1154  static_cast<typename XL320Reg::TYPE_PID_GAIN>(data_list.at(0)));
1155  if (COMM_SUCCESS == res)
1156  break;
1157  }
1158  if (COMM_SUCCESS != res)
1159  return res;
1160 
1161  for (int tries = 10; tries > 0; --tries)
1162  {
1163  res = write<typename XL320Reg::TYPE_PID_GAIN>(XL320Reg::ADDR_POSITION_I_GAIN, id,
1164  static_cast<typename XL320Reg::TYPE_PID_GAIN>(data_list.at(1)));
1165  if (COMM_SUCCESS == res)
1166  break;
1167  }
1168  if (COMM_SUCCESS != res)
1169  return res;
1170 
1171  for (int tries = 10; tries > 0; --tries)
1172  {
1173  res = write<typename XL320Reg::TYPE_PID_GAIN>(XL320Reg::ADDR_POSITION_D_GAIN, id,
1174  static_cast<typename XL320Reg::TYPE_PID_GAIN>(data_list.at(2)));
1175  if (COMM_SUCCESS == res)
1176  break;
1177  }
1178 
1179  return res;
1180  }
1181 
1188  template <>
1189  inline int DxlDriver<XL320Reg>::syncWritePositionGoal(const std::vector<uint8_t> &id_list, const std::vector<uint32_t> &position_list)
1190  {
1191  std::vector<typename XL320Reg::TYPE_GOAL_POSITION> casted_list;
1192  casted_list.reserve(position_list.size());
1193  for (auto const &p : position_list)
1194  {
1195  casted_list.emplace_back(static_cast<typename XL320Reg::TYPE_GOAL_POSITION>(p));
1196  }
1197  return syncWrite<typename XL320Reg::TYPE_GOAL_POSITION>(XL320Reg::ADDR_GOAL_POSITION, id_list, casted_list);
1198  }
1199 
1206  template <>
1207  inline int DxlDriver<XL320Reg>::syncWriteVelocityGoal(const std::vector<uint8_t> &id_list, const std::vector<uint32_t> &velocity_list)
1208  {
1209  std::vector<typename XL320Reg::TYPE_GOAL_VELOCITY> casted_list;
1210  casted_list.reserve(velocity_list.size());
1211  for (auto const &v : velocity_list)
1212  {
1213  casted_list.emplace_back(static_cast<typename XL320Reg::TYPE_GOAL_VELOCITY>(v));
1214  }
1215  return syncWrite<typename XL320Reg::TYPE_GOAL_VELOCITY>(XL320Reg::ADDR_GOAL_VELOCITY, id_list, casted_list);
1216  }
1217 
1218  template <>
1219  inline int DxlDriver<XL320Reg>::readControlMode(uint8_t /*id*/, uint8_t & /*data*/)
1220  {
1221  std::cout << "readControlMode not available for motor XL320" << std::endl;
1222  return COMM_SUCCESS;
1223  }
1224 
1225  // XL430
1226 
1227  template <>
1228  inline std::string DxlDriver<XL430Reg>::interpretErrorState(uint32_t hw_state) const
1229  {
1230  std::string hardware_message;
1231 
1232  if (hw_state & 1 << 0) // 0b00000001
1233  {
1234  hardware_message += "Input Voltage";
1235  }
1236  if (hw_state & 1 << 2) // 0b00000100
1237  {
1238  if (!hardware_message.empty())
1239  hardware_message += ", ";
1240  hardware_message += "OverHeating";
1241  }
1242  if (hw_state & 1 << 3) // 0b00001000
1243  {
1244  if (!hardware_message.empty())
1245  hardware_message += ", ";
1246  hardware_message += "Motor Encoder";
1247  }
1248  if (hw_state & 1 << 4) // 0b00010000
1249  {
1250  if (!hardware_message.empty())
1251  hardware_message += ", ";
1252  hardware_message += "Electrical Shock";
1253  }
1254  if (hw_state & 1 << 5) // 0b00100000
1255  {
1256  if (!hardware_message.empty())
1257  hardware_message += ", ";
1258  hardware_message += "Overload";
1259  }
1260  if (hw_state & 1 << 7) // 0b10000000 => added by us : disconnected error
1261  {
1262  if (!hardware_message.empty())
1263  hardware_message += ", ";
1264  hardware_message += "Disconnection";
1265  }
1266  if (!hardware_message.empty())
1267  hardware_message += " Error";
1268 
1269  return hardware_message;
1270  }
1271 
1272  template <>
1273  inline int DxlDriver<XL430Reg>::writeTorqueGoal(uint8_t /*id*/, uint16_t /*torque*/)
1274  {
1275  std::cout << "writeTorqueGoal not available for motor XL430" << std::endl;
1276  return COMM_TX_ERROR;
1277  }
1278 
1279  template <>
1280  inline int DxlDriver<XL430Reg>::syncWriteTorqueGoal(const std::vector<uint8_t> & /*id_list*/, const std::vector<uint16_t> & /*torque_list*/)
1281  {
1282  std::cout << "syncWriteTorqueGoal not available for motor XL430" << std::endl;
1283  return COMM_TX_ERROR;
1284  }
1285 
1286  // XC430
1287 
1288  template <>
1289  inline std::string DxlDriver<XC430Reg>::interpretErrorState(uint32_t hw_state) const
1290  {
1291  std::string hardware_message;
1292 
1293  if (hw_state & 1 << 0) // 0b00000001
1294  {
1295  hardware_message += "Input Voltage";
1296  }
1297  if (hw_state & 1 << 2) // 0b00000100
1298  {
1299  if (!hardware_message.empty())
1300  hardware_message += ", ";
1301  hardware_message += "OverHeating";
1302  }
1303  if (hw_state & 1 << 3) // 0b00001000
1304  {
1305  if (!hardware_message.empty())
1306  hardware_message += ", ";
1307  hardware_message += "Motor Encoder";
1308  }
1309  if (hw_state & 1 << 4) // 0b00010000
1310  {
1311  if (!hardware_message.empty())
1312  hardware_message += ", ";
1313  hardware_message += "Electrical Shock";
1314  }
1315  if (hw_state & 1 << 5) // 0b00100000
1316  {
1317  if (!hardware_message.empty())
1318  hardware_message += ", ";
1319  hardware_message += "Overload";
1320  }
1321  if (hw_state & 1 << 7) // 0b10000000 => added by us : disconnected error
1322  {
1323  if (!hardware_message.empty())
1324  hardware_message += ", ";
1325  hardware_message += "Disconnection";
1326  }
1327  if (!hardware_message.empty())
1328  hardware_message += " Error";
1329 
1330  return hardware_message;
1331  }
1332 
1333  template <>
1334  inline int DxlDriver<XC430Reg>::writeTorqueGoal(uint8_t /*id*/, uint16_t /*torque*/)
1335  {
1336  std::cout << "writeTorqueGoal not available for motor XC430" << std::endl;
1337  return COMM_TX_ERROR;
1338  }
1339 
1340  template <>
1341  inline int DxlDriver<XC430Reg>::syncWriteTorqueGoal(const std::vector<uint8_t> & /*id_list*/, const std::vector<uint16_t> & /*torque_list*/)
1342  {
1343  std::cout << "syncWriteTorqueGoal not available for motor XC430" << std::endl;
1344  return COMM_TX_ERROR;
1345  }
1346 
1347  // XM430
1348 
1349  template <>
1350  inline std::string DxlDriver<XM430Reg>::interpretErrorState(uint32_t hw_state) const
1351  {
1352  std::string hardware_message;
1353 
1354  if (hw_state & 1 << 0) // 0b00000001
1355  {
1356  hardware_message += "Input Voltage";
1357  }
1358  if (hw_state & 1 << 2) // 0b00000100
1359  {
1360  if (!hardware_message.empty())
1361  hardware_message += ", ";
1362  hardware_message += "OverHeating";
1363  }
1364  if (hw_state & 1 << 3) // 0b00001000
1365  {
1366  if (!hardware_message.empty())
1367  hardware_message += ", ";
1368  hardware_message += "Motor Encoder";
1369  }
1370  if (hw_state & 1 << 4) // 0b00010000
1371  {
1372  if (!hardware_message.empty())
1373  hardware_message += ", ";
1374  hardware_message += "Electrical Shock";
1375  }
1376  if (hw_state & 1 << 5) // 0b00100000
1377  {
1378  if (!hardware_message.empty())
1379  hardware_message += ", ";
1380  hardware_message += "Overload";
1381  }
1382  if (hw_state & 1 << 7) // 0b10000000 => added by us : disconnected error
1383  {
1384  if (!hardware_message.empty())
1385  hardware_message += ", ";
1386  hardware_message += "Disconnection";
1387  }
1388  if (!hardware_message.empty())
1389  hardware_message += " Error";
1390 
1391  return hardware_message;
1392  }
1393 
1394  // works with current instead of load
1395 
1396  template <>
1397  inline int DxlDriver<XM430Reg>::writeTorqueGoal(uint8_t id, uint16_t torque)
1398  {
1399  return write<typename XM430Reg::TYPE_GOAL_CURRENT>(XM430Reg::ADDR_GOAL_CURRENT, id, torque);
1400  }
1401 
1402  template <>
1403  inline int DxlDriver<XM430Reg>::syncWriteTorqueGoal(const std::vector<uint8_t> &id_list, const std::vector<uint16_t> &torque_list)
1404  {
1405  return syncWrite<typename XM430Reg::TYPE_GOAL_CURRENT>(XM430Reg::ADDR_GOAL_CURRENT, id_list, torque_list);
1406  }
1407 
1408  template <>
1409  inline int DxlDriver<XM430Reg>::readLoad(uint8_t id, uint16_t &present_load)
1410  {
1411  return read<typename XM430Reg::TYPE_PRESENT_CURRENT>(XM430Reg::ADDR_PRESENT_CURRENT, id, present_load);
1412  }
1413 
1414  template <>
1415  inline int DxlDriver<XM430Reg>::syncReadLoad(const std::vector<uint8_t> &id_list, std::vector<uint16_t> &load_list)
1416  {
1417  return syncRead<typename XM430Reg::TYPE_PRESENT_CURRENT>(XM430Reg::ADDR_PRESENT_CURRENT, id_list, load_list);
1418  }
1419 
1420  // XL330
1421 
1422  template <>
1423  inline std::string DxlDriver<XL330Reg>::interpretErrorState(uint32_t hw_state) const
1424  {
1425  std::string hardware_message;
1426 
1427  if (hw_state & 1 << 0) // 0b00000001
1428  {
1429  hardware_message += "Input Voltage";
1430  }
1431  if (hw_state & 1 << 2) // 0b00000100
1432  {
1433  if (!hardware_message.empty())
1434  hardware_message += ", ";
1435  hardware_message += "OverHeating";
1436  }
1437  if (hw_state & 1 << 3) // 0b00001000
1438  {
1439  if (!hardware_message.empty())
1440  hardware_message += ", ";
1441  hardware_message += "Motor Encoder";
1442  }
1443  if (hw_state & 1 << 4) // 0b00010000
1444  {
1445  if (!hardware_message.empty())
1446  hardware_message += ", ";
1447  hardware_message += "Electrical Shock";
1448  }
1449  if (hw_state & 1 << 5) // 0b00100000
1450  {
1451  if (!hardware_message.empty())
1452  hardware_message += ", ";
1453  hardware_message += "Overload";
1454  }
1455  if (hw_state & 1 << 7) // 0b10000000 => added by us : disconnected error
1456  {
1457  if (!hardware_message.empty())
1458  hardware_message += ", ";
1459  hardware_message += "Disconnection";
1460  }
1461  if (!hardware_message.empty())
1462  hardware_message += " Error";
1463 
1464  return hardware_message;
1465  }
1466 
1467  // works with current instead of load
1468 
1469  template <>
1470  inline int DxlDriver<XL330Reg>::writeTorqueGoal(uint8_t id, uint16_t torque)
1471  {
1472  return write<typename XL330Reg::TYPE_GOAL_CURRENT>(XL330Reg::ADDR_GOAL_CURRENT, id, torque);
1473  }
1474 
1475  template <>
1476  inline int DxlDriver<XL330Reg>::syncWriteTorqueGoal(const std::vector<uint8_t> &id_list, const std::vector<uint16_t> &torque_list)
1477  {
1478  return syncWrite<typename XL330Reg::TYPE_GOAL_CURRENT>(XL330Reg::ADDR_GOAL_CURRENT, id_list, torque_list);
1479  }
1480 
1481  template <>
1482  inline int DxlDriver<XL330Reg>::readLoad(uint8_t id, uint16_t &present_load)
1483  {
1484  return read<typename XL330Reg::TYPE_PRESENT_CURRENT>(XL330Reg::ADDR_PRESENT_CURRENT, id, present_load);
1485  }
1486 
1487  template <>
1488  inline int DxlDriver<XL330Reg>::syncReadLoad(const std::vector<uint8_t> &id_list, std::vector<uint16_t> &load_list)
1489  {
1490  return syncRead<typename XL330Reg::TYPE_PRESENT_CURRENT>(XL330Reg::ADDR_PRESENT_CURRENT, id_list, load_list);
1491  }
1492 
1493  template <>
1494  inline int DxlDriver<XL330Reg>::writeTemperatureLimit(uint8_t id, uint8_t temperature_limit)
1495  {
1496  return write<typename XL330Reg::TYPE_TEMPERATURE_LIMIT>(XL330Reg::ADDR_TEMPERATURE_LIMIT, id, temperature_limit);
1497  }
1498 
1499  template <>
1500  inline int DxlDriver<XL330Reg>::writeShutdownConfiguration(uint8_t id, uint8_t configuration)
1501  {
1502  return write<typename XL330Reg::TYPE_ALARM_SHUTDOWN>(XL330Reg::ADDR_ALARM_SHUTDOWN, id, configuration);
1503  }
1504 
1505  // XH430
1506 
1507  template <>
1508  inline std::string DxlDriver<XH430Reg>::interpretErrorState(uint32_t hw_state) const
1509  {
1510  std::string hardware_message;
1511 
1512  if (hw_state & 1 << 0) // 0b00000001
1513  {
1514  hardware_message += "Input Voltage";
1515  }
1516  if (hw_state & 1 << 2) // 0b00000100
1517  {
1518  if (!hardware_message.empty())
1519  hardware_message += ", ";
1520  hardware_message += "OverHeating";
1521  }
1522  if (hw_state & 1 << 3) // 0b00001000
1523  {
1524  if (!hardware_message.empty())
1525  hardware_message += ", ";
1526  hardware_message += "Motor Encoder";
1527  }
1528  if (hw_state & 1 << 4) // 0b00010000
1529  {
1530  if (!hardware_message.empty())
1531  hardware_message += ", ";
1532  hardware_message += "Electrical Shock";
1533  }
1534  if (hw_state & 1 << 5) // 0b00100000
1535  {
1536  if (!hardware_message.empty())
1537  hardware_message += ", ";
1538  hardware_message += "Overload";
1539  }
1540  if (hw_state & 1 << 7) // 0b10000000 => added by us : disconnected error
1541  {
1542  if (!hardware_message.empty())
1543  hardware_message += ", ";
1544  hardware_message += "Disconnection";
1545  }
1546  if (!hardware_message.empty())
1547  hardware_message += " Error";
1548 
1549  return hardware_message;
1550  }
1551 
1552  template <>
1553  inline int DxlDriver<XH430Reg>::writeTorqueGoal(uint8_t id, uint16_t torque)
1554  {
1555  return write<typename XL330Reg::TYPE_GOAL_CURRENT>(XL330Reg::ADDR_GOAL_CURRENT, id, torque);
1556  }
1557 
1558  template <>
1559  inline int DxlDriver<XH430Reg>::syncWriteTorqueGoal(const std::vector<uint8_t> & id_list, const std::vector<uint16_t> & torque_list)
1560  {
1561  return syncWrite<typename XL330Reg::TYPE_GOAL_CURRENT>(XL330Reg::ADDR_GOAL_CURRENT, id_list, torque_list);
1562  }
1563 
1564 } // ttl_driver
1565 
1566 #endif // DxlDriver
ttl_driver::DxlDriver::writePositionGoal
int writePositionGoal(uint8_t id, uint32_t position) override
DxlDriver<reg_type>::writePositionGoal.
Definition: dxl_driver.hpp:367
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:685
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:656
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:856
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:465
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:430
ttl_driver::DxlDriver::writeTorquePercentage
int writeTorquePercentage(uint8_t id, uint8_t torque_percentage) override
DxlDriver<reg_type>::writeTorquePercentage.
Definition: dxl_driver.hpp:354
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:737
ttl_driver::DxlDriver::writePID
int writePID(uint8_t id, const std::vector< uint16_t > &data) override
DxlDriver<reg_type>::writePID.
Definition: dxl_driver.hpp:516
ttl_driver::DxlDriver::writeTemperatureLimit
int writeTemperatureLimit(uint8_t id, uint8_t temperature_limit) override
DxlDriver<reg_type>::writeTemperatureLimit.
Definition: dxl_driver.hpp:269
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:713
ttl_driver::DxlDriver::writeTorqueGoal
int writeTorqueGoal(uint8_t id, uint16_t torque) override
DxlDriver<reg_type>::writeTorqueGoal.
Definition: dxl_driver.hpp:725
ttl_driver::DxlDriver::readHwErrorStatus
int readHwErrorStatus(uint8_t id, uint8_t &hardware_error_status) override
DxlDriver<reg_type>::readHwErrorStatus.
Definition: dxl_driver.hpp:492
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:639
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:880
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:892
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:593
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:227
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:282
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:477
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:393
ttl_driver::DxlDriver::readControlMode
int readControlMode(uint8_t id, uint8_t &control_mode) override
DxlDriver<reg_type>::readControlMode.
Definition: dxl_driver.hpp:827
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:805
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:868
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:622
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:751
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:423
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:411
ttl_driver::DxlDriver::writeStartupConfiguration
int writeStartupConfiguration(uint8_t id, uint8_t config) override
DxlDriver<reg_type>::writeStartupConfiguration.
Definition: dxl_driver.hpp:243
ttl_driver::DxlDriver::readLoad
int readLoad(uint8_t id, uint16_t &present_load) override
DxlDriver<reg_type>::readLoad.
Definition: dxl_driver.hpp:844
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:453
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:307
ttl_driver::DxlDriver::writeLed
int writeLed(uint8_t id, uint8_t led_value) override
DxlDriver<reg_type>::writeLed.
Definition: dxl_driver.hpp:701
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:610
ttl_driver::DxlDriver::interpretFirmwareVersion
std::string interpretFirmwareVersion(uint32_t fw_version) const override
DxlDriver<reg_type>::interpretFirmwareVersion.
Definition: dxl_driver.hpp:179
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:295
ttl_driver::DxlDriver::readMoving
int readMoving(uint8_t id, uint8_t &status) override
DxlDriver<reg_type>::readMoving.
Definition: dxl_driver.hpp:793
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:322
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:504
ttl_driver::XL320Reg::ADDR_PRESENT_POSITION
static constexpr uint16_t ADDR_PRESENT_POSITION
Definition: xl320_reg.hpp:98
ttl_driver::DxlDriver::checkModelNumber
int checkModelNumber(uint8_t id) override
DxlDriver<reg_type>::checkModelNumber.
Definition: dxl_driver.hpp:204
ttl_driver::DxlDriver::writeVelocityGoal
int writeVelocityGoal(uint8_t id, uint32_t velocity) override
DxlDriver<reg_type>::writeVelocityGoal.
Definition: dxl_driver.hpp:380
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 Wed May 21 2025 08:30:14