├── Dynamixel.h
├── Dynamixel
    ├── Dynamixel.h
    ├── DynamixelControlTable.h
    └── lib
    │   └── DynamixelSDK
    │       ├── LICENSE
    │       └── include
    │           ├── dynamixel_sdk.h
    │           ├── group_bulk_read.h
    │           ├── group_bulk_write.h
    │           ├── group_sync_read.h
    │           ├── group_sync_write.h
    │           ├── packet_handler.h
    │           ├── port_handler_arduino.h
    │           ├── protocol1_packet_handler.h
    │           ├── protocol2_packet_handler.h
    │           └── types.h
├── LICENSE
├── README.md
├── examples
    ├── WIP_bulk_read_write
    │   └── bulk_read_write.ino
    ├── WIP_indirect_address
    │   └── indirect_address.ino
    ├── WIP_sync_read_write
    │   └── sync_read_write.ino
    ├── broadcast_ping
    │   └── broadcast_ping.ino
    ├── factory_reset
    │   └── factory_reset.ino
    ├── move
    │   └── move.ino
    ├── ping
    │   └── ping.ino
    ├── reboot
    │   └── reboot.ino
    ├── speed
    │   └── speed.ino
    └── two_motors
    │   └── two_motors.ino
├── library.json
└── library.properties


/Dynamixel.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 | 
3 | #ifndef ARDUINO_DYNAMIXEL_H
4 | #define ARDUINO_DYNAMIXEL_H
5 | 
6 | #include "Dynamixel/Dynamixel.h"
7 | 
8 | #endif // ARDUINO_DYNAMIXEL_H
9 | 


--------------------------------------------------------------------------------
/Dynamixel/Dynamixel.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | #ifndef ARDUINO_DYNAMIXEL_IMPL_H
  3 | #define ARDUINO_DYNAMIXEL_IMPL_H
  4 | 
  5 | #include "lib/DynamixelSDK/include/dynamixel_sdk.h"
  6 | #include <ArxSmartPtr.h>
  7 | #include "DynamixelControlTable.h"
  8 | 
  9 | namespace arduino {
 10 | namespace dynamixel {
 11 | 
 12 | enum class ProtocolVersion {
 13 |     V1,
 14 |     V2
 15 | };
 16 | 
 17 | const uint8_t ID_BROADCAST = 0xFE;
 18 | const uint8_t ID_LIMIT = 0xFC;
 19 | 
 20 | class Dynamixel {
 21 |     // TODO: do not use new, make instance inside
 22 |     ::dynamixel::PortHandler* port_handler;
 23 |     ::dynamixel::PacketHandler* packet_handler;
 24 | #if 0
 25 |         ::dynamixel::GroupSyncWrite sync_writer;
 26 |         ::dynamixel::GroupSyncRead sync_reader;
 27 |         ::dynamixel::GroupBulkWrite bulk_writer;
 28 |         ::dynamixel::GroupBulkRead bulk_reader;
 29 | #endif
 30 | 
 31 |     struct Info {
 32 |         std::shared_ptr<ControlTable> ct;
 33 |         int result;
 34 |         uint8_t error;
 35 |         uint16_t model;
 36 |     };
 37 |     Map<uint8_t, Info> info;
 38 | 
 39 | public:
 40 |     using Models = Vec<uint8_t>;
 41 | 
 42 |     // TODO: do not use new, make instance inside
 43 |     Dynamixel(uint8_t pin_rts_enable, ProtocolVersion ver = ProtocolVersion::V2)
 44 |         : port_handler(new ::dynamixel::PortHandler(pin_rts_enable)), packet_handler((ver == ProtocolVersion::V2) ? (::dynamixel::PacketHandler*)new ::dynamixel::Protocol2PacketHandler() : (::dynamixel::PacketHandler*)new ::dynamixel::Protocol1PacketHandler())
 45 | #if 0
 46 |         , sync_writer(port_handler, packet_handler)
 47 |         , sync_reader(port_handler, packet_handler)
 48 |         , bulk_writer(port_handler, packet_handler)
 49 |         , bulk_reader(port_handler, packet_handler)
 50 | #endif
 51 |     {
 52 |     }
 53 | 
 54 |     Dynamixel(uint8_t pin_rx_enable, uint8_t pin_tx_enable, ProtocolVersion ver = ProtocolVersion::V2)
 55 |         : port_handler(new ::dynamixel::PortHandler(pin_rx_enable, pin_tx_enable)), packet_handler((ver == ProtocolVersion::V2) ? (::dynamixel::PacketHandler*)new ::dynamixel::Protocol2PacketHandler() : (::dynamixel::PacketHandler*)new ::dynamixel::Protocol1PacketHandler())
 56 | #if 0
 57 |         , sync_writer(port_handler, packet_handler)
 58 |         , sync_reader(port_handler, packet_handler)
 59 |         , bulk_writer(port_handler, packet_handler)
 60 |         , bulk_reader(port_handler, packet_handler)
 61 | #endif
 62 |     {
 63 |     }
 64 | 
 65 |     ~Dynamixel() {
 66 |         delete port_handler;
 67 |         delete packet_handler;
 68 |     }
 69 | 
 70 |     template <Model>
 71 |     void addModel(uint8_t id) {
 72 |         std::shared_ptr<ControlTable> sp = ControlTableOfModel<Model::OTHER>::instance();
 73 |         info.emplace(id, Info{sp, 0, 0, 0});
 74 |     }
 75 | 
 76 |     void attach(Stream& s, size_t baud) {
 77 |         port_handler->attach(s, baud);
 78 |         packet_handler->attach(port_handler);
 79 |     }
 80 | 
 81 |     uint8_t size() const { return info.size(); }
 82 | 
 83 |     // wrapper for instructions
 84 | 
 85 |     bool write(uint8_t id, Reg reg, uint32_t data) {
 86 |         if (info.find(id) == info.end()) return false;
 87 | 
 88 |         uint16_t addr = info[id].ct->ct[reg].addr;
 89 |         uint8_t size = info[id].ct->ct[reg].size;
 90 |         uint8_t error;
 91 |         int result = packet_handler->writeBytesTxRx(id, addr, data, size, &error);
 92 |         return handleResult(id, result, error);
 93 |     }
 94 | 
 95 |     uint32_t read(uint8_t id, Reg reg) {
 96 |         if (info.find(id) == info.end()) return 0xFFFFFFFF;
 97 | 
 98 |         uint32_t value = 0;
 99 |         uint16_t addr = info[id].ct->ct[reg].addr;
100 |         uint8_t size = info[id].ct->ct[reg].size;
101 |         uint8_t error = 0;
102 |         int result = packet_handler->readBytesTxRx(id, addr, (uint8_t*)&value, size, &error);
103 |         bool b = handleResult(id, result, error);
104 |         return b ? value : 0xFFFFFFFF;
105 |     }
106 | 
107 |     bool ping(uint8_t id) {
108 |         if (id > ID_LIMIT) return false;
109 |         uint8_t error = 0;
110 |         uint16_t model = 0;
111 |         int result = packet_handler->ping(id, &model, &error);
112 |         return handleResult(id, result, error, model);
113 |     }
114 | 
115 |     Models ping()  // broadcast
116 |     {
117 |         Models ids;
118 |         int result = packet_handler->broadcastPing(ids);
119 |         if (result != COMM_SUCCESS) return Models();
120 |         return ids;
121 |     }
122 | 
123 |     bool factoryReset(uint8_t id, ResetMode mode = ResetMode::EXC_ID_BAUD) {
124 |         uint8_t error = 0;
125 |         int result = packet_handler->factoryReset(id, (uint8_t)mode, &error);
126 |         return handleResult(id, result, error);
127 |     }
128 | 
129 |     bool factoryReset(ResetMode mode = ResetMode::EXC_ID_BAUD) {
130 |         return factoryReset(ID_BROADCAST, mode);
131 |     }
132 | 
133 |     bool reboot(uint8_t id) {
134 |         uint8_t error = 0;
135 |         int result = packet_handler->reboot(id, &error);
136 |         return handleResult(id, result, error);
137 |     }
138 | 
139 |     void verbose(uint8_t id) {
140 |         if (info.find(id) == info.end()) return;
141 |         verboseResult(id);
142 |         verboseError(id);
143 |     }
144 | 
145 |     // TODO: if there is no such ID
146 |     int lastCommResult(uint8_t id) { return (info.find(id) != info.end()) ? info[id].result : -1; }
147 |     uint8_t lastError(uint8_t id) { return (info.find(id) != info.end()) ? info[id].error : 0; }
148 |     uint16_t lastModelNo(uint8_t id) { return (info.find(id) != info.end()) ? info[id].model : 0; }
149 | 
150 |     // wrappers for control table
151 | 
152 |     // read values
153 |     uint16_t modelNumber(uint8_t id) { return (uint16_t)read(id, Reg::MODEL_NUMBER); }
154 |     uint32_t modelInformation(uint8_t id) { return (uint32_t)read(id, Reg::MODEL_INFORMATION); }
155 |     uint8_t versionOfFirmware(uint8_t id) { return (uint8_t)read(id, Reg::VERSION_OF_FIRMWARE); }
156 |     uint8_t id(uint8_t id) { return (uint8_t)read(id, Reg::ID); }
157 |     uint8_t baudrate(uint8_t id) { return (uint8_t)read(id, Reg::BAUDRATE); }
158 |     uint8_t returnDelayTime(uint8_t id) { return (uint8_t)read(id, Reg::RETURN_DELAY_TIME); }
159 |     uint8_t driveMode(uint8_t id) { return (uint8_t)read(id, Reg::DRIVE_MODE); }
160 |     uint8_t operatingMode(uint8_t id) { return (uint8_t)read(id, Reg::OPERATING_MODE); }
161 |     uint8_t secondaryId(uint8_t id) { return (uint8_t)read(id, Reg::SECONDARY_ID); }
162 |     uint8_t protocolVersion(uint8_t id) { return (uint8_t)read(id, Reg::PROTOCOL_VERSION); }
163 |     int32_t homingOffset(uint8_t id) { return (int32_t)read(id, Reg::HOMING_OFFSET); }
164 |     uint32_t movingThreshold(uint8_t id) { return (uint32_t)read(id, Reg::MOVING_THRESHOLD); }
165 |     uint8_t temperatureLimit(uint8_t id) { return (uint8_t)read(id, Reg::TEMPERATURE_LIMIT); }
166 |     uint16_t maxVoltageLimit(uint8_t id) { return (uint16_t)read(id, Reg::MAX_VOLTAGE_LIMIT); }
167 |     uint16_t minVoltageLimit(uint8_t id) { return (uint16_t)read(id, Reg::MIN_VOLTAGE_LIMIT); }
168 |     uint16_t pwmLimit(uint8_t id) { return (uint16_t)read(id, Reg::PWM_LIMIT); }
169 |     uint16_t currentLimit(uint8_t id) { return (uint16_t)read(id, Reg::CURRENT_LIMIT); }
170 |     uint32_t accelerationLimit(uint8_t id) { return (uint32_t)read(id, Reg::ACCELERATION_LIMIT); }
171 |     uint32_t velocityLimit(uint8_t id) { return (uint32_t)read(id, Reg::VELOCITY_LIMIT); }
172 |     uint32_t maxPositionLimit(uint8_t id) { return (uint32_t)read(id, Reg::MAX_POSITION_LIMIT); }
173 |     uint32_t minPositionLimit(uint8_t id) { return (uint32_t)read(id, Reg::MIN_POSITION_LIMIT); }
174 |     uint8_t shutdown(uint8_t id) { return (uint8_t)read(id, Reg::SHUTDOWN); }
175 |     bool torqueEnable(uint8_t id) { return (bool)read(id, Reg::TORQUE_ENABLE); }
176 |     uint8_t led(uint8_t id) { return (uint8_t)read(id, Reg::LED); }
177 |     uint8_t statusReturnLevel(uint8_t id) { return (uint8_t)read(id, Reg::STATUS_RETURN_LEVEL); }
178 |     uint8_t registerdInstruction(uint8_t id) { return (uint8_t)read(id, Reg::REGISTERED_INSTRUCTION); }
179 |     uint8_t hardwareErrorStatus(uint8_t id) { return (uint8_t)read(id, Reg::HARDWARE_ERROR_STATUS); }
180 |     uint16_t velocityIGain(uint8_t id) { return (uint16_t)read(id, Reg::VELOCITY_I_GAIN); }
181 |     uint16_t velocityPGain(uint8_t id) { return (uint16_t)read(id, Reg::VELOCITY_P_GAIN); }
182 |     uint16_t positionDGain(uint8_t id) { return (uint16_t)read(id, Reg::POSITION_D_GAIN); }
183 |     uint16_t positionIGain(uint8_t id) { return (uint16_t)read(id, Reg::POSITION_I_GAIN); }
184 |     uint16_t positionPGain(uint8_t id) { return (uint16_t)read(id, Reg::POSITION_P_GAIN); }
185 |     uint16_t feedForwardAccelerationGain(uint8_t id) { return (uint16_t)read(id, Reg::FEEDFORWARD_ACCELERATION_GAIN); }
186 |     uint16_t feedForwardVelocityGain(uint8_t id) { return (uint16_t)read(id, Reg::FEEDFORWARD_VELOCITY_GAIN); }
187 |     int8_t busWatchdog(uint8_t id) { return (int8_t)read(id, Reg::BUS_WATCHDOG); }
188 |     int16_t goalPwm(uint8_t id) { return (int16_t)read(id, Reg::GOAL_PWM); }
189 |     int16_t goalCurrent(uint8_t id) { return (int16_t)read(id, Reg::GOAL_CURRENT); }
190 |     int32_t goalVelocity(uint8_t id) { return (int32_t)read(id, Reg::GOAL_VELOCITY); }
191 |     uint32_t profileAcceleration(uint8_t id) { return (uint32_t)read(id, Reg::PROFILE_ACCELERATION); }
192 |     uint32_t profileVelocity(uint8_t id) { return (uint32_t)read(id, Reg::PROFILE_VELOCITY); }
193 |     int32_t goalPosition(uint8_t id) { return (int32_t)read(id, Reg::GOAL_POSITION); }
194 |     uint16_t realTimeTick(uint8_t id) { return (uint16_t)read(id, Reg::REALTIME_TICK); }
195 |     uint8_t moving(uint8_t id) { return (uint8_t)read(id, Reg::MOVING); }
196 |     uint8_t movingStatus(uint8_t id) { return (uint8_t)read(id, Reg::MOVING_STATUS); }
197 |     int16_t presentPwm(uint8_t id) { return (int16_t)read(id, Reg::PRESENT_PWM); }
198 |     int16_t presentCurrent(uint8_t id) { return (int16_t)read(id, Reg::PRESENT_CURRENT); }
199 |     int32_t presentVelocity(uint8_t id) { return (int32_t)read(id, Reg::PRESENT_VELOCITY); }
200 |     int32_t presentPosition(uint8_t id) { return (int32_t)read(id, Reg::PRESENT_POSITION); }
201 |     uint32_t velocityTrajectory(uint8_t id) { return (uint32_t)read(id, Reg::VELOCITY_TRAJECTORY); }
202 |     uint32_t positionTrajectory(uint8_t id) { return (uint32_t)read(id, Reg::POSITION_TRAJECTORY); }
203 |     uint16_t presentInputVoltage(uint8_t id) { return (uint16_t)read(id, Reg::PRESENT_INPUT_VOLTAGE); }
204 |     uint8_t presentTemperature(uint8_t id) { return (uint8_t)read(id, Reg::PRESENT_TEMPERATURE); }
205 | 
206 |     // write values
207 |     bool id(uint8_t id, uint8_t x) { return write(id, Reg::ID, x); }
208 |     bool baudrate(uint8_t id, uint8_t x) { return write(id, Reg::BAUDRATE, x); }
209 |     bool returnDelayTime(uint8_t id, uint8_t x) { return write(id, Reg::RETURN_DELAY_TIME, x); }
210 |     bool driveMode(uint8_t id, uint8_t x) { return write(id, Reg::DRIVE_MODE, x); }
211 |     bool operatingMode(uint8_t id, uint8_t x) { return write(id, Reg::OPERATING_MODE, x); }
212 |     bool secondaryId(uint8_t id, uint8_t x) { return write(id, Reg::SECONDARY_ID, x); }
213 |     bool protocolVersion(uint8_t id, uint8_t x) { return write(id, Reg::PROTOCOL_VERSION, x); }
214 |     bool homingOffset(uint8_t id, int32_t x) { return write(id, Reg::HOMING_OFFSET, x); }
215 |     bool movingThreshold(uint8_t id, uint32_t x) { return write(id, Reg::MOVING_THRESHOLD, x); }
216 |     bool temperatureLimit(uint8_t id, uint8_t x) { return write(id, Reg::TEMPERATURE_LIMIT, x); }
217 |     bool maxVoltageLimit(uint8_t id, uint16_t x) { return write(id, Reg::MAX_VOLTAGE_LIMIT, x); }
218 |     bool minVoltageLimit(uint8_t id, uint16_t x) { return write(id, Reg::MIN_VOLTAGE_LIMIT, x); }
219 |     bool pwmLimit(uint8_t id, uint16_t x) { return write(id, Reg::PWM_LIMIT, x); }
220 |     bool currentLimit(uint8_t id, uint16_t x) { return write(id, Reg::CURRENT_LIMIT, x); }
221 |     bool accelerationLimit(uint8_t id, uint32_t x) { return write(id, Reg::ACCELERATION_LIMIT, x); }
222 |     bool velocityLimit(uint8_t id, uint32_t x) { return write(id, Reg::VELOCITY_LIMIT, x); }
223 |     bool maxPositionLimit(uint8_t id, uint32_t x) { return write(id, Reg::MAX_POSITION_LIMIT, x); }
224 |     bool minPositionLimit(uint8_t id, uint32_t x) { return write(id, Reg::MIN_POSITION_LIMIT, x); }
225 |     bool shutdown(uint8_t id, uint8_t x) { return write(id, Reg::SHUTDOWN, x); }
226 |     bool torqueEnable(uint8_t id, bool x) { return write(id, Reg::TORQUE_ENABLE, x); }
227 |     bool led(uint8_t id, bool x) { return write(id, Reg::LED, x); }
228 |     bool statusReturnLevel(uint8_t id, uint8_t x) { return write(id, Reg::STATUS_RETURN_LEVEL, x); }
229 |     bool velocityIGain(uint8_t id, uint16_t x) { return write(id, Reg::VELOCITY_I_GAIN, x); }
230 |     bool velocityPGain(uint8_t id, uint16_t x) { return write(id, Reg::VELOCITY_P_GAIN, x); }
231 |     bool positionDGain(uint8_t id, uint16_t x) { return write(id, Reg::POSITION_D_GAIN, x); }
232 |     bool positionIGain(uint8_t id, uint16_t x) { return write(id, Reg::POSITION_I_GAIN, x); }
233 |     bool positionPGain(uint8_t id, uint16_t x) { return write(id, Reg::POSITION_P_GAIN, x); }
234 |     bool feedForwardAccelerationGain(uint8_t id, uint16_t x) { return write(id, Reg::FEEDFORWARD_ACCELERATION_GAIN, x); }
235 |     bool feedForwardVelocityGain(uint8_t id, uint16_t x) { return write(id, Reg::FEEDFORWARD_VELOCITY_GAIN, x); }
236 |     bool busWatchdog(uint8_t id, int8_t x) { return write(id, Reg::BUS_WATCHDOG, x); }
237 |     bool goalPwm(uint8_t id, int16_t x) { return write(id, Reg::GOAL_PWM, x); }
238 |     bool goalCurrent(uint8_t id, int16_t x) { return write(id, Reg::GOAL_CURRENT, x); }
239 |     bool goalVelocity(uint8_t id, int32_t x) { return write(id, Reg::GOAL_VELOCITY, x); }
240 |     bool profileAcceleration(uint8_t id, uint32_t x) { return write(id, Reg::PROFILE_ACCELERATION, x); }
241 |     bool profileVelocity(uint8_t id, uint32_t x) { return write(id, Reg::PROFILE_VELOCITY, x); }
242 |     bool goalPosition(uint8_t id, int32_t x) { return write(id, Reg::GOAL_POSITION, x); }
243 | 
244 | private:
245 |     bool handleResult(uint8_t id, uint8_t result, uint8_t error = 0, uint16_t model = 0) {
246 |         bool b = true;
247 |         if ((result != COMM_SUCCESS) || (error != 0)) b = false;
248 |         saveResult(id, result, error, model);
249 |         return b;
250 |     }
251 | 
252 |     void saveResult(uint8_t id, int result, uint8_t error = 0, uint16_t model = 0) {
253 |         if (id > ID_LIMIT) return;
254 | 
255 |         auto it = info.find(id);
256 |         if (it == info.end()) return;
257 | 
258 |         it->second.result = result;
259 |         it->second.error = error;
260 |         if (model != 0) it->second.model = model;
261 |     }
262 | 
263 |     void verboseResult(uint8_t id) {
264 |         if (info[id].result != COMM_SUCCESS)
265 |             Serial.println(packet_handler->getTxRxResult(info[id].result));
266 |     }
267 |     void verboseError(uint8_t id) {
268 |         if (info[id].error != 0)
269 |             Serial.println(packet_handler->getRxPacketError(info[id].error));
270 |     }
271 | 
272 | #if 0   // TODO: reg_write & action, indirect, sync, bulk
273 |         bool reg_write(uint8_t id, uint16_t reg, uint16_t size, uint8_t* data)
274 |         {
275 |             // TODO: clamp value
276 |             uint8_t error = 0;
277 |             int result = packet_handler->regWriteTxRx(id, reg, size, data, &error);
278 |             return handleResult(id, result, error);
279 |         }
280 |         bool action(uint8_t id)
281 |         {
282 |             int result = packet_handler->action(id);
283 |             return handleResult(id, result);
284 |         }
285 |         // for indirect address
286 |         bool setIndirectAddress(uint8_t id, uint8_t nth, uint16_t addr)
287 |         {
288 |             if (nth > 128) return false;
289 |             uint8_t error = 0;
290 |             // int result = packet_handler->write2ByteTxRx(id, info[id].ct->ct[Reg::INDIRECT_ADDR_1].addr + 2 * nth, addr, &error);
291 |             int result = packet_handler->writeBytesTxRx(id, info[id].ct->ct[Reg::INDIRECT_ADDR_1].addr + 2 * nth, addr, info[id].ct->ct[Reg::INDIRECT_ADDR_1].size, &error);
292 |             return handleResult(id, result, error);
293 |         }
294 |         bool setIndirectData(uint8_t id, uint8_t nth, uint8_t data)
295 |         {
296 |             uint8_t error = 0;
297 |             // TODO: ayashii data size
298 |             // int result = packet_handler->write2ByteTxRx(id, info[id].ct->ct[Reg::INDIRECT_DATA_1].addr + nth, data, &error);
299 |             int result = packet_handler->writeBytesTxRx(id, info[id].ct->ct[Reg::INDIRECT_DATA_1].addr + nth, data, info[id].ct->ct[Reg::INDIRECT_DATA_1].size, &error);
300 |             return handleResult(id, result, error);
301 |         }
302 |         // for sync write
303 |         void set_address(uint16_t addr, uint16_t size)
304 |         {
305 |             sync_writer.setAddress(addr, size);
306 |         }
307 |         bool add(uint8_t id, uint8_t* data)
308 |         {
309 |             // TODO: check if endian is ok or not
310 |             int result = sync_writer.addParam(id, data);
311 |             if (result != true) return false;
312 |             return true;
313 |         }
314 |         bool send()
315 |         {
316 |             int result = sync_writer.txPacket();
317 |             sync_writer.clearParam();
318 |             if (result != COMM_SUCCESS) return false;
319 |             return true;
320 |         }
321 |         // for sync read
322 |         // TODO: duplicate name with sync write
323 |         void set_target(uint16_t addr, uint16_t size)
324 |         {
325 |             sync_reader.setAddress(addr, size);
326 |         }
327 |         // TODO: varidic arguments...
328 |         bool add_id(uint8_t id)
329 |         {
330 |             int result = sync_reader.addParam(id);
331 |             if (result != true) return false;
332 |             return true;
333 |         }
334 |         bool request()
335 |         {
336 |             int result = sync_reader.txRxPacket();
337 |             if (result != COMM_SUCCESS) return false;
338 |             return true;
339 |         }
340 |         bool available(uint8_t id)
341 |         {
342 |             return sync_reader.isAvailable(id);
343 |         }
344 |         uint32_t data(uint8_t id)
345 |         {
346 |             return sync_reader.getData(id);
347 |         }
348 |         // for bulk write
349 |         bool add_bulk_target(uint8_t id, uint16_t addr, uint16_t size, uint8_t* data)
350 |         {
351 |             return bulk_writer.addParam(id, addr, size, data);
352 |         }
353 |         bool bulk_write()
354 |         {
355 |             int result = bulk_writer.txPacket();
356 |             bulk_writer.clearParam();
357 |             if (result != COMM_SUCCESS) return false;
358 |             return true;
359 |         }
360 |         // for bulk read
361 |         bool add_bulk_read_target(uint8_t id, uint16_t addr, uint16_t size)
362 |         {
363 |             return bulk_reader.addParam(id, addr, size);
364 |         }
365 |         bool bulk_read_request()
366 |         {
367 |             int result = bulk_reader.txRxPacket();
368 |             if (result != COMM_SUCCESS) return false;
369 |             return true;
370 |         }
371 |         bool bulk_available(uint8_t id)
372 |         {
373 |             return bulk_reader.isAvailable(id);
374 |         }
375 |         uint32_t bulk_read_data(uint8_t id)
376 |         {
377 |             return bulk_reader.getData(id);
378 |         }
379 | #endif  // TODO:
380 | };
381 | 
382 | template <>
383 | inline void Dynamixel::addModel<Model::PRO>(uint8_t id) {
384 |     std::shared_ptr<ControlTable> sp = ControlTableOfModel<Model::PRO>::instance();
385 |     info.emplace(id, Dynamixel::Info{sp, 0, 0, 0});
386 | }
387 | template <>
388 | inline void Dynamixel::addModel<Model::X>(uint8_t id) {
389 |     std::shared_ptr<ControlTable> sp = ControlTableOfModel<Model::X>::instance();
390 |     info.emplace(id, Dynamixel::Info{sp, 0, 0, 0});
391 | }
392 | template <>
393 | inline void Dynamixel::addModel<Model::MX>(uint8_t id) {
394 |     std::shared_ptr<ControlTable> sp = ControlTableOfModel<Model::MX>::instance();
395 |     info.emplace(id, Dynamixel::Info{sp, 0, 0, 0});
396 | }
397 | 
398 | }  // namespace dynamixel
399 | }  // namespace arduino
400 | 
401 | using Dynamixel = arduino::dynamixel::Dynamixel;
402 | using DxlModel = arduino::dynamixel::Model;
403 | using DxlReg = arduino::dynamixel::Reg;
404 | 
405 | #endif  // ARDUINO_DYNAMIXEL_IMPL_H
406 | 


--------------------------------------------------------------------------------
/Dynamixel/DynamixelControlTable.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | #ifndef DYNAMIXEL_CONTROL_TABLE_H
  3 | #define DYNAMIXEL_CONTROL_TABLE_H
  4 | 
  5 | #include <ArxSmartPtr.h>
  6 | 
  7 | namespace arduino {
  8 | namespace dynamixel {
  9 | 
 10 | #if ARX_HAVE_LIBSTDCPLUSPLUS >= 201103L  // Have libstdc++11
 11 | template <typename T>
 12 | using Vec = std::vector<T>;
 13 | template <typename T, typename U>
 14 | using Map = std::map<T, U>;
 15 | #else  // Do not have libstdc++11
 16 | template <typename T>
 17 | using Vec = arx::stdx::vector<T>;
 18 | template <typename T, typename U>
 19 | using Map = arx::stdx::map<T, U>;
 20 | #endif
 21 | 
 22 | enum class Model {
 23 |     PRO,
 24 |     X,
 25 |     MX,
 26 |     OTHER
 27 | };
 28 | 
 29 | enum class Reg {
 30 |     // EEPROM
 31 |     MODEL_NUMBER,
 32 |     MODEL_INFORMATION,
 33 |     VERSION_OF_FIRMWARE,
 34 |     ID,
 35 |     BAUDRATE,
 36 |     RETURN_DELAY_TIME,
 37 |     DRIVE_MODE,
 38 |     OPERATING_MODE,
 39 |     SECONDARY_ID,
 40 |     PROTOCOL_VERSION,
 41 |     HOMING_OFFSET,
 42 |     MOVING_THRESHOLD,
 43 |     TEMPERATURE_LIMIT,
 44 |     MAX_VOLTAGE_LIMIT,
 45 |     MIN_VOLTAGE_LIMIT,
 46 |     PWM_LIMIT,
 47 |     CURRENT_LIMIT,
 48 |     ACCELERATION_LIMIT,
 49 |     VELOCITY_LIMIT,
 50 |     MAX_POSITION_LIMIT,
 51 |     MIN_POSITION_LIMIT,
 52 |     SHUTDOWN,
 53 |     // RAM
 54 |     TORQUE_ENABLE,
 55 |     LED,
 56 |     STATUS_RETURN_LEVEL,
 57 |     REGISTERED_INSTRUCTION,
 58 |     HARDWARE_ERROR_STATUS,
 59 |     VELOCITY_I_GAIN,
 60 |     VELOCITY_P_GAIN,
 61 |     POSITION_D_GAIN,
 62 |     POSITION_I_GAIN,
 63 |     POSITION_P_GAIN,
 64 |     FEEDFORWARD_ACCELERATION_GAIN,
 65 |     FEEDFORWARD_VELOCITY_GAIN,
 66 |     BUS_WATCHDOG,
 67 |     GOAL_PWM,
 68 |     GOAL_CURRENT,
 69 |     GOAL_VELOCITY,
 70 |     PROFILE_ACCELERATION,
 71 |     PROFILE_VELOCITY,
 72 |     GOAL_POSITION,
 73 |     REALTIME_TICK,
 74 |     MOVING,
 75 |     MOVING_STATUS,
 76 |     PRESENT_PWM,
 77 |     PRESENT_CURRENT,
 78 |     PRESENT_VELOCITY,
 79 |     PRESENT_POSITION,
 80 |     VELOCITY_TRAJECTORY,
 81 |     POSITION_TRAJECTORY,
 82 | 
 83 |     // TBD
 84 | 
 85 |     PRESENT_INPUT_VOLTAGE,
 86 |     PRESENT_TEMPERATURE,
 87 |     INDIRECT_ADDR_1,
 88 |     INDIRECT_DATA_1,
 89 |     INDIRECT_ADDR_29,
 90 |     INDIRECT_DATA_29,
 91 | 
 92 |     // EEPROM (OTHERS)
 93 |     // CW_ANGLE_LIMIT,
 94 |     // CCW_ANGLE_LIMIT,
 95 |     TORQUE_MAX,
 96 |     ALARM_LED,
 97 |     ALARM_SHUTDOWN,
 98 |     MULTI_TURN_OFFSET,
 99 |     RESOLUTION_DIVIDER,
100 |     TORQUE_LIMIT,
101 |     EEPROM_LOCK,
102 |     PUNCH,
103 |     GOAL_ACCELERATION,
104 | 
105 |     EXTERNAL_PORT_MODE_1,
106 |     EXTERNAL_PORT_MODE_2,
107 |     EXTERNAL_PORT_MODE_3,
108 |     EXTERNAL_PORT_MODE_4,
109 |     EXTERNAL_PORT_DATA_1,
110 |     EXTERNAL_PORT_DATA_2,
111 |     EXTERNAL_PORT_DATA_3,
112 |     EXTERNAL_PORT_DATA_4,
113 |     LED_R,
114 |     LED_G,
115 |     LED_B,
116 |     GOAL_TORQUE,
117 |     PRESENT_LOAD,
118 | 
119 |     DOWN_CALIBRATION,
120 |     UP_CALIBRATION,
121 |     CW_COMPLIANCE_MARGIN,
122 |     CCW_COMPLIANCE_MARGIN,
123 |     CW_COMPLIANCE_SLOPE,
124 |     CCW_COMPLIANCE_SLOPE,
125 | 
126 | };
127 | 
128 | struct RegInfo {
129 |     uint16_t addr;
130 |     uint8_t size;
131 | };
132 | 
133 | struct ControlTable {
134 |     Map<Reg, RegInfo> ct;
135 | };
136 | 
137 | template <Model>  // Model::OTHER
138 | class ControlTableOfModel : public ControlTable {
139 | public:
140 |     static std::shared_ptr<ControlTableOfModel> instance() {
141 |         static std::shared_ptr<ControlTableOfModel> instance(new ControlTableOfModel());
142 |         return instance;
143 |     }
144 | 
145 | private:
146 |     ControlTableOfModel(const ControlTableOfModel&) = delete;
147 |     ControlTableOfModel& operator=(const ControlTableOfModel&) = delete;
148 |     ControlTableOfModel() {
149 |         Serial.println(F("OTHER series Control Table"));
150 |         ct = Map<Reg, RegInfo>{
151 |             // only for Protocol 1.0
152 |             // EEPROM
153 |             {Reg::MODEL_NUMBER, {0, 2}},
154 |             {Reg::VERSION_OF_FIRMWARE, {2, 1}},
155 |             {Reg::ID, {3, 1}},
156 |             {Reg::BAUDRATE, {4, 1}},
157 |             {Reg::RETURN_DELAY_TIME, {5, 1}},
158 |             {Reg::MIN_POSITION_LIMIT, {6, 2}},
159 |             {Reg::MAX_POSITION_LIMIT, {8, 2}},
160 |             {Reg::TEMPERATURE_LIMIT, {11, 1}},
161 |             {Reg::MIN_VOLTAGE_LIMIT, {12, 1}},
162 |             {Reg::MAX_VOLTAGE_LIMIT, {13, 1}},
163 |             {Reg::TORQUE_MAX, {14, 2}},
164 |             {Reg::STATUS_RETURN_LEVEL, {16, 1}},
165 |             {Reg::ALARM_LED, {17, 1}},
166 |             {Reg::ALARM_SHUTDOWN, {18, 1}},
167 |             {Reg::DOWN_CALIBRATION, {20, 2}},
168 |             {Reg::UP_CALIBRATION, {22, 2}},
169 |             {Reg::TORQUE_ENABLE, {24, 1}},
170 |             {Reg::LED, {25, 1}},
171 |             {Reg::CW_COMPLIANCE_MARGIN, {26, 1}},
172 |             {Reg::CCW_COMPLIANCE_MARGIN, {27, 1}},
173 |             {Reg::CW_COMPLIANCE_SLOPE, {28, 1}},
174 |             {Reg::CCW_COMPLIANCE_SLOPE, {29, 1}},
175 |             {Reg::GOAL_POSITION, {30, 2}},
176 |             {Reg::VELOCITY_LIMIT, {32, 2}},
177 |             {Reg::TORQUE_LIMIT, {34, 2}},
178 |             {Reg::PRESENT_POSITION, {36, 2}},
179 |             {Reg::PRESENT_VELOCITY, {38, 2}},
180 |             {Reg::PRESENT_LOAD, {40, 2}},
181 |             {Reg::PRESENT_INPUT_VOLTAGE, {42, 1}},
182 |             {Reg::PRESENT_TEMPERATURE, {43, 1}},
183 |             {Reg::REGISTERED_INSTRUCTION, {44, 1}},
184 |             {Reg::MOVING, {46, 1}},
185 |             {Reg::EEPROM_LOCK, {47, 1}},
186 |             {Reg::PUNCH, {48, 2}},
187 |         };
188 |     }
189 | };
190 | 
191 | template <>
192 | class ControlTableOfModel<Model::PRO> : public ControlTable {
193 | public:
194 |     static std::shared_ptr<ControlTableOfModel> instance() {
195 |         static std::shared_ptr<ControlTableOfModel> instance(new ControlTableOfModel());
196 |         return instance;
197 |     }
198 | 
199 | private:
200 |     ControlTableOfModel(const ControlTableOfModel&) = delete;
201 |     ControlTableOfModel& operator=(const ControlTableOfModel&) = delete;
202 |     ControlTableOfModel() {
203 |         Serial.println(F("PRO series Control Table"));
204 |         ct = Map<Reg, RegInfo>{
205 |             {Reg::MODEL_NUMBER, {0, 2}},
206 |             {Reg::MODEL_INFORMATION, {2, 4}},
207 |             {Reg::VERSION_OF_FIRMWARE, {6, 1}},
208 |             {Reg::ID, {7, 1}},
209 |             {Reg::BAUDRATE, {8, 1}},
210 |             {Reg::RETURN_DELAY_TIME, {9, 1}},
211 |             {Reg::OPERATING_MODE, {11, 1}},
212 |             {Reg::HOMING_OFFSET, {13, 4}},
213 |             {Reg::MOVING_THRESHOLD, {17, 4}},
214 |             {Reg::TEMPERATURE_LIMIT, {21, 1}},
215 |             {Reg::MAX_VOLTAGE_LIMIT, {22, 2}},
216 |             {Reg::MIN_VOLTAGE_LIMIT, {24, 2}},
217 |             {Reg::ACCELERATION_LIMIT, {26, 4}},
218 |             {Reg::TORQUE_LIMIT, {30, 2}},
219 |             {Reg::VELOCITY_LIMIT, {32, 4}},
220 |             {Reg::MAX_POSITION_LIMIT, {36, 4}},
221 |             {Reg::MIN_POSITION_LIMIT, {40, 4}},
222 |             {Reg::EXTERNAL_PORT_MODE_1, {44, 1}},
223 |             {Reg::EXTERNAL_PORT_MODE_2, {45, 1}},
224 |             {Reg::EXTERNAL_PORT_MODE_3, {46, 1}},
225 |             {Reg::EXTERNAL_PORT_MODE_4, {47, 1}},
226 |             {Reg::SHUTDOWN, {48, 1}},
227 |             {Reg::INDIRECT_ADDR_1, {49, 2}},
228 |             {Reg::TORQUE_ENABLE, {562, 1}},
229 |             {Reg::LED_R, {563, 1}},
230 |             {Reg::LED_G, {564, 1}},
231 |             {Reg::LED_B, {565, 1}},
232 |             {Reg::VELOCITY_I_GAIN, {586, 2}},
233 |             {Reg::VELOCITY_P_GAIN, {588, 2}},
234 |             {Reg::POSITION_P_GAIN, {594, 2}},
235 |             {Reg::GOAL_POSITION, {596, 4}},
236 |             {Reg::GOAL_VELOCITY, {600, 4}},
237 |             {Reg::GOAL_TORQUE, {604, 2}},
238 |             {Reg::GOAL_ACCELERATION, {606, 4}},
239 |             {Reg::MOVING, {610, 1}},
240 |             {Reg::PRESENT_POSITION, {611, 4}},
241 |             {Reg::PRESENT_VELOCITY, {615, 4}},
242 |             {Reg::PRESENT_CURRENT, {621, 2}},
243 |             {Reg::PRESENT_INPUT_VOLTAGE, {623, 2}},
244 |             {Reg::PRESENT_TEMPERATURE, {625, 1}},
245 |             {Reg::EXTERNAL_PORT_DATA_1, {626, 2}},
246 |             {Reg::EXTERNAL_PORT_DATA_2, {628, 2}},
247 |             {Reg::EXTERNAL_PORT_DATA_3, {630, 2}},
248 |             {Reg::EXTERNAL_PORT_DATA_4, {632, 2}},
249 |             {Reg::INDIRECT_DATA_1, {634, 1}},
250 |             {Reg::REGISTERED_INSTRUCTION, {890, 1}},
251 |             {Reg::STATUS_RETURN_LEVEL, {891, 1}},
252 |             {Reg::HARDWARE_ERROR_STATUS, {892, 1}},
253 |         };
254 |     }
255 | };
256 | 
257 | template <>
258 | class ControlTableOfModel<Model::X> : public ControlTable {
259 | public:
260 |     static std::shared_ptr<ControlTableOfModel> instance() {
261 |         static std::shared_ptr<ControlTableOfModel> instance(new ControlTableOfModel());
262 |         return instance;
263 |     }
264 | 
265 | private:
266 |     ControlTableOfModel(const ControlTableOfModel&) = delete;
267 |     ControlTableOfModel& operator=(const ControlTableOfModel&) = delete;
268 |     ControlTableOfModel() {
269 |         Serial.println(F("X series Control Table"));
270 |         ct = Map<Reg, RegInfo>{
271 |             // EEPROM
272 |             {Reg::MODEL_NUMBER, {0, 2}},
273 |             {Reg::MODEL_INFORMATION, {2, 4}},
274 |             {Reg::VERSION_OF_FIRMWARE, {6, 1}},
275 |             {Reg::ID, {7, 1}},
276 |             {Reg::BAUDRATE, {8, 1}},
277 |             {Reg::RETURN_DELAY_TIME, {9, 1}},
278 |             {Reg::DRIVE_MODE, {10, 1}},
279 |             {Reg::OPERATING_MODE, {11, 1}},
280 |             {Reg::SECONDARY_ID, {12, 1}},
281 |             {Reg::PROTOCOL_VERSION, {13, 1}},
282 |             {Reg::HOMING_OFFSET, {20, 4}},
283 |             {Reg::MOVING_THRESHOLD, {24, 4}},
284 |             {Reg::TEMPERATURE_LIMIT, {31, 1}},
285 |             {Reg::MAX_VOLTAGE_LIMIT, {32, 2}},
286 |             {Reg::MIN_VOLTAGE_LIMIT, {34, 2}},
287 |             {Reg::PWM_LIMIT, {36, 2}},
288 |             {Reg::CURRENT_LIMIT, {38, 2}},
289 |             {Reg::ACCELERATION_LIMIT, {40, 4}},
290 |             {Reg::VELOCITY_LIMIT, {44, 4}},
291 |             {Reg::MAX_POSITION_LIMIT, {48, 4}},
292 |             {Reg::MIN_POSITION_LIMIT, {52, 4}},
293 |             {Reg::SHUTDOWN, {63, 4}},
294 |             // RAM
295 |             {Reg::TORQUE_ENABLE, {64, 1}},
296 |             {Reg::LED, {65, 1}},
297 |             {Reg::STATUS_RETURN_LEVEL, {68, 1}},
298 |             {Reg::REGISTERED_INSTRUCTION, {69, 1}},  // 0: no instruction, 1: reg_write && !action
299 |             {Reg::HARDWARE_ERROR_STATUS, {70, 1}},
300 |             {Reg::VELOCITY_I_GAIN, {76, 2}},
301 |             {Reg::VELOCITY_P_GAIN, {78, 2}},
302 |             {Reg::POSITION_D_GAIN, {80, 2}},
303 |             {Reg::POSITION_I_GAIN, {82, 2}},
304 |             {Reg::POSITION_P_GAIN, {84, 2}},
305 |             {Reg::FEEDFORWARD_ACCELERATION_GAIN, {88, 2}},
306 |             {Reg::FEEDFORWARD_VELOCITY_GAIN, {90, 2}},
307 |             {Reg::BUS_WATCHDOG, {98, 1}},
308 |             {Reg::GOAL_PWM, {100, 2}},
309 |             {Reg::GOAL_CURRENT, {102, 2}},
310 |             {Reg::GOAL_VELOCITY, {104, 4}},
311 |             {Reg::PROFILE_ACCELERATION, {108, 4}},
312 |             {Reg::PROFILE_VELOCITY, {112, 4}},
313 |             {Reg::GOAL_POSITION, {116, 4}},
314 |             {Reg::REALTIME_TICK, {120, 2}},
315 |             {Reg::MOVING, {122, 1}},
316 |             {Reg::MOVING_STATUS, {123, 1}},
317 |             {Reg::PRESENT_PWM, {124, 2}},
318 |             {Reg::PRESENT_CURRENT, {126, 2}},
319 |             {Reg::PRESENT_VELOCITY, {128, 4}},
320 |             {Reg::PRESENT_POSITION, {132, 4}},
321 |             {Reg::VELOCITY_TRAJECTORY, {136, 4}},
322 |             {Reg::POSITION_TRAJECTORY, {140, 4}},
323 |             {Reg::PRESENT_INPUT_VOLTAGE, {144, 2}},
324 |             {Reg::PRESENT_TEMPERATURE, {146, 1}},
325 |             {Reg::INDIRECT_ADDR_1, {168, 2}},
326 |             {Reg::INDIRECT_DATA_1, {224, 1}},
327 |             {Reg::INDIRECT_ADDR_29, {578, 2}},
328 |             {Reg::INDIRECT_DATA_29, {634, 1}}};
329 |     }
330 | };
331 | 
332 | template <>
333 | class ControlTableOfModel<Model::MX> : public ControlTable {
334 | public:
335 |     static std::shared_ptr<ControlTableOfModel> instance() {
336 |         static std::shared_ptr<ControlTableOfModel> instance(new ControlTableOfModel());
337 |         return instance;
338 |     }
339 | 
340 | private:
341 |     ControlTableOfModel(const ControlTableOfModel&) = delete;
342 |     ControlTableOfModel& operator=(const ControlTableOfModel&) = delete;
343 |     ControlTableOfModel() {
344 |         Serial.println(F("MX series Control Table"));
345 |         ct = Map<Reg, RegInfo>{
346 |             // only for Protocol 2.0
347 |             // EEPROM
348 |             {Reg::MODEL_NUMBER, {0, 2}},
349 |             {Reg::MODEL_INFORMATION, {2, 4}},
350 |             {Reg::VERSION_OF_FIRMWARE, {6, 1}},
351 |             {Reg::ID, {7, 1}},
352 |             {Reg::BAUDRATE, {8, 1}},
353 |             {Reg::RETURN_DELAY_TIME, {9, 1}},
354 |             {Reg::DRIVE_MODE, {10, 1}},
355 |             {Reg::OPERATING_MODE, {11, 1}},
356 |             {Reg::SECONDARY_ID, {12, 1}},
357 |             {Reg::PROTOCOL_VERSION, {13, 1}},
358 |             {Reg::HOMING_OFFSET, {20, 4}},
359 |             {Reg::MOVING_THRESHOLD, {24, 4}},
360 |             {Reg::TEMPERATURE_LIMIT, {31, 1}},
361 |             {Reg::MAX_VOLTAGE_LIMIT, {32, 2}},
362 |             {Reg::MIN_VOLTAGE_LIMIT, {34, 2}},
363 |             {Reg::PWM_LIMIT, {36, 2}},
364 |             {Reg::CURRENT_LIMIT, {38, 2}},
365 |             {Reg::ACCELERATION_LIMIT, {40, 4}},
366 |             {Reg::VELOCITY_LIMIT, {44, 4}},
367 |             {Reg::MAX_POSITION_LIMIT, {48, 4}},
368 |             {Reg::MIN_POSITION_LIMIT, {52, 4}},
369 |             {Reg::SHUTDOWN, {63, 4}},
370 |             // RAM
371 |             {Reg::TORQUE_ENABLE, {64, 1}},
372 |             {Reg::LED, {65, 1}},
373 |             {Reg::STATUS_RETURN_LEVEL, {68, 1}},
374 |             {Reg::REGISTERED_INSTRUCTION, {69, 1}},  // 0: no instruction, 1: reg_write && !action
375 |             {Reg::HARDWARE_ERROR_STATUS, {70, 1}},
376 |             {Reg::VELOCITY_I_GAIN, {76, 2}},
377 |             {Reg::VELOCITY_P_GAIN, {78, 2}},
378 |             {Reg::POSITION_D_GAIN, {80, 2}},
379 |             {Reg::POSITION_I_GAIN, {82, 2}},
380 |             {Reg::POSITION_P_GAIN, {84, 2}},
381 |             {Reg::FEEDFORWARD_ACCELERATION_GAIN, {88, 2}},
382 |             {Reg::FEEDFORWARD_VELOCITY_GAIN, {90, 2}},
383 |             {Reg::BUS_WATCHDOG, {98, 1}},
384 |             {Reg::GOAL_PWM, {100, 2}},
385 |             {Reg::GOAL_CURRENT, {102, 2}},
386 |             {Reg::GOAL_VELOCITY, {104, 4}},
387 |             {Reg::PROFILE_ACCELERATION, {108, 4}},
388 |             {Reg::PROFILE_VELOCITY, {112, 4}},
389 |             {Reg::GOAL_POSITION, {116, 4}},
390 |             {Reg::REALTIME_TICK, {120, 2}},
391 |             {Reg::MOVING, {122, 1}},
392 |             {Reg::MOVING_STATUS, {123, 1}},
393 |             {Reg::PRESENT_PWM, {124, 2}},
394 |             {Reg::PRESENT_CURRENT, {126, 2}},
395 |             {Reg::PRESENT_VELOCITY, {128, 4}},
396 |             {Reg::PRESENT_POSITION, {132, 4}},
397 |             {Reg::VELOCITY_TRAJECTORY, {136, 4}},
398 |             {Reg::POSITION_TRAJECTORY, {140, 4}},
399 |             {Reg::PRESENT_INPUT_VOLTAGE, {144, 2}},
400 |             {Reg::PRESENT_TEMPERATURE, {146, 1}},
401 |             {Reg::EXTERNAL_PORT_DATA_1, {152, 2}},
402 |             {Reg::EXTERNAL_PORT_DATA_2, {154, 2}},
403 |             {Reg::EXTERNAL_PORT_DATA_3, {156, 2}},
404 |             {Reg::INDIRECT_ADDR_1, {168, 2}},
405 |             {Reg::INDIRECT_DATA_1, {224, 1}},
406 |             {Reg::INDIRECT_ADDR_29, {578, 2}},
407 |             {Reg::INDIRECT_DATA_29, {634, 1}}};
408 |     }
409 | };
410 | 
411 | enum class SatusReturnLevel {
412 |     EXC_PING,
413 |     EXC_PING_READ,
414 |     ALL
415 | };
416 | enum class ResetMode {
417 |     ALL = 0xFF,
418 |     EXC_ID = 0x01,
419 |     EXC_ID_BAUD = 0x02
420 | };
421 | 
422 | }  // namespace dynamixel
423 | }  // namespace arduino
424 | 
425 | #endif  // DYNAMIXEL_CONTROL_TABLE_H
426 | 


--------------------------------------------------------------------------------
/Dynamixel/lib/DynamixelSDK/LICENSE:
--------------------------------------------------------------------------------
  1 |                                  Apache License
  2 |                            Version 2.0, January 2004
  3 |                         http://www.apache.org/licenses/
  4 | 
  5 |    TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
  6 | 
  7 |    1. Definitions.
  8 | 
  9 |       "License" shall mean the terms and conditions for use, reproduction,
 10 |       and distribution as defined by Sections 1 through 9 of this document.
 11 | 
 12 |       "Licensor" shall mean the copyright owner or entity authorized by
 13 |       the copyright owner that is granting the License.
 14 | 
 15 |       "Legal Entity" shall mean the union of the acting entity and all
 16 |       other entities that control, are controlled by, or are under common
 17 |       control with that entity. For the purposes of this definition,
 18 |       "control" means (i) the power, direct or indirect, to cause the
 19 |       direction or management of such entity, whether by contract or
 20 |       otherwise, or (ii) ownership of fifty percent (50%) or more of the
 21 |       outstanding shares, or (iii) beneficial ownership of such entity.
 22 | 
 23 |       "You" (or "Your") shall mean an individual or Legal Entity
 24 |       exercising permissions granted by this License.
 25 | 
 26 |       "Source" form shall mean the preferred form for making modifications,
 27 |       including but not limited to software source code, documentation
 28 |       source, and configuration files.
 29 | 
 30 |       "Object" form shall mean any form resulting from mechanical
 31 |       transformation or translation of a Source form, including but
 32 |       not limited to compiled object code, generated documentation,
 33 |       and conversions to other media types.
 34 | 
 35 |       "Work" shall mean the work of authorship, whether in Source or
 36 |       Object form, made available under the License, as indicated by a
 37 |       copyright notice that is included in or attached to the work
 38 |       (an example is provided in the Appendix below).
 39 | 
 40 |       "Derivative Works" shall mean any work, whether in Source or Object
 41 |       form, that is based on (or derived from) the Work and for which the
 42 |       editorial revisions, annotations, elaborations, or other modifications
 43 |       represent, as a whole, an original work of authorship. For the purposes
 44 |       of this License, Derivative Works shall not include works that remain
 45 |       separable from, or merely link (or bind by name) to the interfaces of,
 46 |       the Work and Derivative Works thereof.
 47 | 
 48 |       "Contribution" shall mean any work of authorship, including
 49 |       the original version of the Work and any modifications or additions
 50 |       to that Work or Derivative Works thereof, that is intentionally
 51 |       submitted to Licensor for inclusion in the Work by the copyright owner
 52 |       or by an individual or Legal Entity authorized to submit on behalf of
 53 |       the copyright owner. For the purposes of this definition, "submitted"
 54 |       means any form of electronic, verbal, or written communication sent
 55 |       to the Licensor or its representatives, including but not limited to
 56 |       communication on electronic mailing lists, source code control systems,
 57 |       and issue tracking systems that are managed by, or on behalf of, the
 58 |       Licensor for the purpose of discussing and improving the Work, but
 59 |       excluding communication that is conspicuously marked or otherwise
 60 |       designated in writing by the copyright owner as "Not a Contribution."
 61 | 
 62 |       "Contributor" shall mean Licensor and any individual or Legal Entity
 63 |       on behalf of whom a Contribution has been received by Licensor and
 64 |       subsequently incorporated within the Work.
 65 | 
 66 |    2. Grant of Copyright License. Subject to the terms and conditions of
 67 |       this License, each Contributor hereby grants to You a perpetual,
 68 |       worldwide, non-exclusive, no-charge, royalty-free, irrevocable
 69 |       copyright license to reproduce, prepare Derivative Works of,
 70 |       publicly display, publicly perform, sublicense, and distribute the
 71 |       Work and such Derivative Works in Source or Object form.
 72 | 
 73 |    3. Grant of Patent License. Subject to the terms and conditions of
 74 |       this License, each Contributor hereby grants to You a perpetual,
 75 |       worldwide, non-exclusive, no-charge, royalty-free, irrevocable
 76 |       (except as stated in this section) patent license to make, have made,
 77 |       use, offer to sell, sell, import, and otherwise transfer the Work,
 78 |       where such license applies only to those patent claims licensable
 79 |       by such Contributor that are necessarily infringed by their
 80 |       Contribution(s) alone or by combination of their Contribution(s)
 81 |       with the Work to which such Contribution(s) was submitted. If You
 82 |       institute patent litigation against any entity (including a
 83 |       cross-claim or counterclaim in a lawsuit) alleging that the Work
 84 |       or a Contribution incorporated within the Work constitutes direct
 85 |       or contributory patent infringement, then any patent licenses
 86 |       granted to You under this License for that Work shall terminate
 87 |       as of the date such litigation is filed.
 88 | 
 89 |    4. Redistribution. You may reproduce and distribute copies of the
 90 |       Work or Derivative Works thereof in any medium, with or without
 91 |       modifications, and in Source or Object form, provided that You
 92 |       meet the following conditions:
 93 | 
 94 |       (a) You must give any other recipients of the Work or
 95 |           Derivative Works a copy of this License; and
 96 | 
 97 |       (b) You must cause any modified files to carry prominent notices
 98 |           stating that You changed the files; and
 99 | 
100 |       (c) You must retain, in the Source form of any Derivative Works
101 |           that You distribute, all copyright, patent, trademark, and
102 |           attribution notices from the Source form of the Work,
103 |           excluding those notices that do not pertain to any part of
104 |           the Derivative Works; and
105 | 
106 |       (d) If the Work includes a "NOTICE" text file as part of its
107 |           distribution, then any Derivative Works that You distribute must
108 |           include a readable copy of the attribution notices contained
109 |           within such NOTICE file, excluding those notices that do not
110 |           pertain to any part of the Derivative Works, in at least one
111 |           of the following places: within a NOTICE text file distributed
112 |           as part of the Derivative Works; within the Source form or
113 |           documentation, if provided along with the Derivative Works; or,
114 |           within a display generated by the Derivative Works, if and
115 |           wherever such third-party notices normally appear. The contents
116 |           of the NOTICE file are for informational purposes only and
117 |           do not modify the License. You may add Your own attribution
118 |           notices within Derivative Works that You distribute, alongside
119 |           or as an addendum to the NOTICE text from the Work, provided
120 |           that such additional attribution notices cannot be construed
121 |           as modifying the License.
122 | 
123 |       You may add Your own copyright statement to Your modifications and
124 |       may provide additional or different license terms and conditions
125 |       for use, reproduction, or distribution of Your modifications, or
126 |       for any such Derivative Works as a whole, provided Your use,
127 |       reproduction, and distribution of the Work otherwise complies with
128 |       the conditions stated in this License.
129 | 
130 |    5. Submission of Contributions. Unless You explicitly state otherwise,
131 |       any Contribution intentionally submitted for inclusion in the Work
132 |       by You to the Licensor shall be under the terms and conditions of
133 |       this License, without any additional terms or conditions.
134 |       Notwithstanding the above, nothing herein shall supersede or modify
135 |       the terms of any separate license agreement you may have executed
136 |       with Licensor regarding such Contributions.
137 | 
138 |    6. Trademarks. This License does not grant permission to use the trade
139 |       names, trademarks, service marks, or product names of the Licensor,
140 |       except as required for reasonable and customary use in describing the
141 |       origin of the Work and reproducing the content of the NOTICE file.
142 | 
143 |    7. Disclaimer of Warranty. Unless required by applicable law or
144 |       agreed to in writing, Licensor provides the Work (and each
145 |       Contributor provides its Contributions) on an "AS IS" BASIS,
146 |       WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
147 |       implied, including, without limitation, any warranties or conditions
148 |       of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
149 |       PARTICULAR PURPOSE. You are solely responsible for determining the
150 |       appropriateness of using or redistributing the Work and assume any
151 |       risks associated with Your exercise of permissions under this License.
152 | 
153 |    8. Limitation of Liability. In no event and under no legal theory,
154 |       whether in tort (including negligence), contract, or otherwise,
155 |       unless required by applicable law (such as deliberate and grossly
156 |       negligent acts) or agreed to in writing, shall any Contributor be
157 |       liable to You for damages, including any direct, indirect, special,
158 |       incidental, or consequential damages of any character arising as a
159 |       result of this License or out of the use or inability to use the
160 |       Work (including but not limited to damages for loss of goodwill,
161 |       work stoppage, computer failure or malfunction, or any and all
162 |       other commercial damages or losses), even if such Contributor
163 |       has been advised of the possibility of such damages.
164 | 
165 |    9. Accepting Warranty or Additional Liability. While redistributing
166 |       the Work or Derivative Works thereof, You may choose to offer,
167 |       and charge a fee for, acceptance of support, warranty, indemnity,
168 |       or other liability obligations and/or rights consistent with this
169 |       License. However, in accepting such obligations, You may act only
170 |       on Your own behalf and on Your sole responsibility, not on behalf
171 |       of any other Contributor, and only if You agree to indemnify,
172 |       defend, and hold each Contributor harmless for any liability
173 |       incurred by, or claims asserted against, such Contributor by reason
174 |       of your accepting any such warranty or additional liability.
175 | 
176 |    END OF TERMS AND CONDITIONS
177 | 
178 |    APPENDIX: How to apply the Apache License to your work.
179 | 
180 |       To apply the Apache License to your work, attach the following
181 |       boilerplate notice, with the fields enclosed by brackets "{}"
182 |       replaced with your own identifying information. (Don't include
183 |       the brackets!)  The text should be enclosed in the appropriate
184 |       comment syntax for the file format. We also recommend that a
185 |       file or class name and description of purpose be included on the
186 |       same "printed page" as the copyright notice for easier
187 |       identification within third-party archives.
188 | 
189 |    Copyright {yyyy} {name of copyright owner}
190 | 
191 |    Licensed under the Apache License, Version 2.0 (the "License");
192 |    you may not use this file except in compliance with the License.
193 |    You may obtain a copy of the License at
194 | 
195 |        http://www.apache.org/licenses/LICENSE-2.0
196 | 
197 |    Unless required by applicable law or agreed to in writing, software
198 |    distributed under the License is distributed on an "AS IS" BASIS,
199 |    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
200 |    See the License for the specific language governing permissions and
201 |    limitations under the License.
202 | 


--------------------------------------------------------------------------------
/Dynamixel/lib/DynamixelSDK/include/dynamixel_sdk.h:
--------------------------------------------------------------------------------
 1 | // 2019.04.01 by Hideaki Tai : simplified to adapt only to Arduino
 2 | /*******************************************************************************
 3 | * Copyright 2017 ROBOTIS CO., LTD.
 4 | *
 5 | * Licensed under the Apache License, Version 2.0 (the "License");
 6 | * you may not use this file except in compliance with the License.
 7 | * You may obtain a copy of the License at
 8 | *
 9 | *     http://www.apache.org/licenses/LICENSE-2.0
10 | *
11 | * Unless required by applicable law or agreed to in writing, software
12 | * distributed under the License is distributed on an "AS IS" BASIS,
13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 | * See the License for the specific language governing permissions and
15 | * limitations under the License.
16 | *******************************************************************************/
17 | 
18 | ////////////////////////////////////////////////////////////////////////////////
19 | /// @file The file that includes whole Dynamixel SDK libraries
20 | /// @author Zerom, Leon (RyuWoon Jung)
21 | ////////////////////////////////////////////////////////////////////////////////
22 | 
23 | #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_
24 | #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_
25 | 
26 | #include "group_bulk_read.h"
27 | #include "group_bulk_write.h"
28 | #include "group_sync_read.h"
29 | #include "group_sync_write.h"
30 | #include "packet_handler.h"
31 | #include "port_handler_arduino.h"
32 | #include "protocol1_packet_handler.h"
33 | #include "protocol2_packet_handler.h"
34 | 
35 | #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ */
36 | 


--------------------------------------------------------------------------------
/Dynamixel/lib/DynamixelSDK/include/group_bulk_read.h:
--------------------------------------------------------------------------------
  1 | // 2019.04.01 by Hideaki Tai : simplified to adapt only to Arduino
  2 | /*******************************************************************************
  3 | * Copyright 2017 ROBOTIS CO., LTD.
  4 | *
  5 | * Licensed under the Apache License, Version 2.0 (the "License");
  6 | * you may not use this file except in compliance with the License.
  7 | * You may obtain a copy of the License at
  8 | *
  9 | *     http://www.apache.org/licenses/LICENSE-2.0
 10 | *
 11 | * Unless required by applicable law or agreed to in writing, software
 12 | * distributed under the License is distributed on an "AS IS" BASIS,
 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 14 | * See the License for the specific language governing permissions and
 15 | * limitations under the License.
 16 | *******************************************************************************/
 17 | 
 18 | ////////////////////////////////////////////////////////////////////////////////
 19 | /// @file The file for Dynamixel Bulk Read
 20 | /// @author Zerom, Leon (RyuWoon Jung)
 21 | ////////////////////////////////////////////////////////////////////////////////
 22 | 
 23 | #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_
 24 | #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_
 25 | 
 26 | #include <ArxTypeTraits.h>
 27 | #include <ArxContainer.h>
 28 | #include "types.h"
 29 | #include "port_handler_arduino.h"
 30 | #include "packet_handler.h"
 31 | 
 32 | namespace dynamixel {
 33 | 
 34 | ////////////////////////////////////////////////////////////////////////////////
 35 | /// @brief The class for reading multiple Dynamixel data from different addresses with different lengths at once
 36 | ////////////////////////////////////////////////////////////////////////////////
 37 | class GroupBulkRead {
 38 |     static constexpr size_t TXPACKET_MAX_LEN = 1 * 1024;
 39 |     static constexpr size_t RXPACKET_MAX_LEN = 1 * 1024;
 40 | 
 41 |     ///////////////// for Protocol 2.0 Packet /////////////////
 42 |     static constexpr uint8_t PKT_HEADER0 = 0;
 43 |     static constexpr uint8_t PKT_HEADER1 = 1;
 44 |     static constexpr uint8_t PKT_HEADER2 = 2;
 45 |     static constexpr uint8_t PKT_RESERVED = 3;
 46 |     static constexpr uint8_t PKT_ID = 4;
 47 |     static constexpr uint8_t PKT_LENGTH_L = 5;
 48 |     static constexpr uint8_t PKT_LENGTH_H = 6;
 49 |     static constexpr uint8_t PKT_INSTRUCTION = 7;
 50 |     static constexpr uint8_t PKT_ERROR = 8;
 51 |     static constexpr uint8_t PKT_PARAMETER0 = 8;
 52 | 
 53 |     ///////////////// Protocol 2.0 Error bit /////////////////
 54 |     static constexpr uint8_t ERRNUM_RESULT_FAIL = 1;  // Failed to process the instruction packet.
 55 |     static constexpr uint8_t ERRNUM_INSTRUCTION = 2;  // Instruction error
 56 |     static constexpr uint8_t ERRNUM_CRC = 3;          // CRC check error
 57 |     static constexpr uint8_t ERRNUM_DATA_RANGE = 4;   // Data range error
 58 |     static constexpr uint8_t ERRNUM_DATA_LENGTH = 5;  // Data length error
 59 |     static constexpr uint8_t ERRNUM_DATA_LIMIT = 6;   // Data limit error
 60 |     static constexpr uint8_t ERRNUM_ACCESS = 7;       // Access error
 61 |     static constexpr uint8_t ERRBIT_ALERT = 128;      //When the device has a problem, this bit is set to 1. Check "Device Status Check" value.
 62 | private:
 63 |     PortHandler *port_;
 64 |     PacketHandler *ph_;
 65 | 
 66 |     Vec<uint8_t> id_list_;
 67 |     Map<uint8_t, uint16_t> address_list_;  // <id, start_address>
 68 |     Map<uint8_t, uint16_t> length_list_;   // <id, data_length>
 69 |     Map<uint8_t, uint8_t *> data_list_;    // <id, data>
 70 |     Map<uint8_t, uint8_t *> error_list_;   // <id, error>
 71 | 
 72 |     bool last_result_;
 73 |     bool is_param_changed_;
 74 | 
 75 |     uint8_t *param_;
 76 | 
 77 |     void makeParam() {
 78 |         if (id_list_.size() == 0)
 79 |             return;
 80 | 
 81 |         if (param_ != 0)
 82 |             delete[] param_;
 83 |         param_ = 0;
 84 | 
 85 |         param_ = new uint8_t[id_list_.size() * 5];  // ID(1) + ADDR(2) + LENGTH(2)
 86 | 
 87 |         int idx = 0;
 88 |         for (unsigned int i = 0; i < id_list_.size(); i++) {
 89 |             uint8_t id = id_list_[i];
 90 |             param_[idx++] = id;                             // ID
 91 |             param_[idx++] = DXL_LOBYTE(address_list_[id]);  // ADDR_L
 92 |             param_[idx++] = DXL_HIBYTE(address_list_[id]);  // ADDR_H
 93 |             param_[idx++] = DXL_LOBYTE(length_list_[id]);   // LEN_L
 94 |             param_[idx++] = DXL_HIBYTE(length_list_[id]);   // LEN_H
 95 |         }
 96 |     }
 97 | 
 98 | public:
 99 |     ////////////////////////////////////////////////////////////////////////////////
100 |     /// @brief The function that Initializes instance for Bulk Read
101 |     /// @param port PortHandler instance
102 |     /// @param ph PacketHandler instance
103 |     ////////////////////////////////////////////////////////////////////////////////
104 |     GroupBulkRead(PortHandler *port, PacketHandler *ph)
105 |         : port_(port),
106 |           ph_(ph),
107 |           last_result_(false),
108 |           is_param_changed_(false),
109 |           param_(0) {
110 |         clearParam();
111 |     }
112 | 
113 |     ////////////////////////////////////////////////////////////////////////////////
114 |     /// @brief The function that calls clearParam function to clear the parameter list for Bulk Read
115 |     ////////////////////////////////////////////////////////////////////////////////
116 |     ~GroupBulkRead() { clearParam(); }
117 | 
118 |     ////////////////////////////////////////////////////////////////////////////////
119 |     /// @brief The function that returns PortHandler instance
120 |     /// @return PortHandler instance
121 |     ////////////////////////////////////////////////////////////////////////////////
122 |     PortHandler *getPortHandler() { return port_; }
123 | 
124 |     ////////////////////////////////////////////////////////////////////////////////
125 |     /// @brief The function that returns PacketHandler instance
126 |     /// @return PacketHandler instance
127 |     ////////////////////////////////////////////////////////////////////////////////
128 |     PacketHandler *getPacketHandler() { return ph_; }
129 | 
130 |     ////////////////////////////////////////////////////////////////////////////////
131 |     /// @brief The function that adds id, start_address, data_length to the Bulk Read list
132 |     /// @param id Dynamixel ID
133 |     /// @param start_address Address of the data for read
134 |     /// @data_length Length of the data for read
135 |     /// @return false
136 |     /// @return   when the ID exists already in the list
137 |     /// @return or true
138 |     ////////////////////////////////////////////////////////////////////////////////
139 |     bool addParam(uint8_t id, uint16_t start_address, uint16_t data_length) {
140 |         // if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end())   // id already exist
141 |         //   return false;
142 |         // if id is already exist, return false
143 |         for (const auto &i : id_list_)  // if id is already exist
144 |             if (i == id) return false;
145 | 
146 |         id_list_.push_back(id);
147 |         length_list_[id] = data_length;
148 |         address_list_[id] = start_address;
149 |         data_list_[id] = new uint8_t[data_length];
150 |         error_list_[id] = new uint8_t[1];
151 | 
152 |         is_param_changed_ = true;
153 |         return true;
154 |     }
155 | 
156 |     ////////////////////////////////////////////////////////////////////////////////
157 |     /// @brief The function that removes id from the Bulk Read list
158 |     /// @param id Dynamixel ID
159 |     ////////////////////////////////////////////////////////////////////////////////
160 |     void removeParam(uint8_t id) {
161 |         // if id is NOT exist, return
162 |         Vec<uint8_t>::iterator it = id_list_.begin();
163 |         for (; it != id_list_.end(); ++it)
164 |             if (*it == id) break;
165 |         if (it == id_list_.end()) return;
166 | 
167 |         id_list_.erase(it);
168 |         address_list_.erase(id);
169 |         length_list_.erase(id);
170 |         delete[] data_list_[id];
171 |         delete[] error_list_[id];
172 |         data_list_.erase(id);
173 |         error_list_.erase(id);
174 | 
175 |         is_param_changed_ = true;
176 |     }
177 | 
178 |     ////////////////////////////////////////////////////////////////////////////////
179 |     /// @brief The function that clears the Bulk Read list
180 |     ////////////////////////////////////////////////////////////////////////////////
181 |     void clearParam() {
182 |         if (id_list_.size() == 0)
183 |             return;
184 | 
185 |         for (unsigned int i = 0; i < id_list_.size(); i++) {
186 |             delete[] data_list_[id_list_[i]];
187 |             delete[] error_list_[id_list_[i]];
188 |         }
189 | 
190 |         id_list_.clear();
191 |         address_list_.clear();
192 |         length_list_.clear();
193 |         data_list_.clear();
194 |         error_list_.clear();
195 |         if (param_ != 0)
196 |             delete[] param_;
197 |         param_ = 0;
198 |     }
199 | 
200 |     ////////////////////////////////////////////////////////////////////////////////
201 |     /// @brief The function that transmits the Bulk Read instruction packet which might be constructed by GroupBulkRead::addParam function
202 |     /// @return COMM_NOT_AVAILABLE
203 |     /// @return   when the list for Bulk Read is empty
204 |     /// @return or the other communication results which come from PacketHandler::bulkReadTx
205 |     ////////////////////////////////////////////////////////////////////////////////
206 |     int txPacket() {
207 |         if (id_list_.size() == 0)
208 |             return COMM_NOT_AVAILABLE;
209 | 
210 |         if (is_param_changed_ == true || param_ == 0)
211 |             makeParam();
212 | 
213 |         return bulkReadTx(port_, param_, id_list_.size() * 5);
214 |     }
215 | 
216 |     ////////////////////////////////////////////////////////////////////////////////
217 |     /// @brief The function that receives the packet which might be come from the Dynamixel
218 |     /// @return COMM_NOT_AVAILABLE
219 |     /// @return   when the list for Bulk Read is empty
220 |     /// @return COMM_RX_FAIL
221 |     /// @return   when there is no packet recieved
222 |     /// @return COMM_SUCCESS
223 |     /// @return   when there is packet recieved
224 |     /// @return or the other communication results
225 |     ////////////////////////////////////////////////////////////////////////////////
226 |     int rxPacket() {
227 |         int cnt = id_list_.size();
228 |         int result = COMM_RX_FAIL;
229 | 
230 |         last_result_ = false;
231 | 
232 |         if (cnt == 0)
233 |             return COMM_NOT_AVAILABLE;
234 | 
235 |         for (int i = 0; i < cnt; i++) {
236 |             uint8_t id = id_list_[i];
237 | 
238 |             result = ph_->readRx(id, length_list_[id], data_list_[id], error_list_[id]);
239 |             if (result != COMM_SUCCESS)
240 |                 return result;
241 |         }
242 | 
243 |         if (result == COMM_SUCCESS)
244 |             last_result_ = true;
245 | 
246 |         return result;
247 |     }
248 | 
249 |     ////////////////////////////////////////////////////////////////////////////////
250 |     /// @brief The function that transmits and receives the packet which might be come from the Dynamixel
251 |     /// @return COMM_RX_FAIL
252 |     /// @return   when there is no packet recieved
253 |     /// @return COMM_SUCCESS
254 |     /// @return   when there is packet recieved
255 |     /// @return or the other communication results which come from GroupBulkRead::txPacket or GroupBulkRead::rxPacket
256 |     ////////////////////////////////////////////////////////////////////////////////
257 |     int txRxPacket() {
258 |         int result = COMM_TX_FAIL;
259 | 
260 |         result = txPacket();
261 |         if (result != COMM_SUCCESS)
262 |             return result;
263 | 
264 |         return rxPacket();
265 |     }
266 | 
267 |     ////////////////////////////////////////////////////////////////////////////////
268 |     /// @brief The function that checks whether there are available data which might be received by GroupBulkRead::rxPacket or GroupBulkRead::txRxPacket
269 |     /// @param id Dynamixel ID
270 |     /// @param address Address of the data for read
271 |     /// @param data_length Length of the data for read
272 |     /// @return false
273 |     /// @return  when there are no data available
274 |     /// @return or true
275 |     ////////////////////////////////////////////////////////////////////////////////
276 |     //   bool        isAvailable (uint8_t id, uint16_t address, uint16_t data_length);
277 |     bool isAvailable(uint8_t id) {
278 |         //   uint16_t start_addr;
279 | 
280 |         if (last_result_ == false || data_list_.find(id) == data_list_.end())
281 |             return false;
282 | 
283 |         //   start_addr = address_list_[id];
284 | 
285 |         //   if (address < start_addr || start_addr + length_list_[id] - data_length < address)
286 |         //     return false;
287 | 
288 |         return true;
289 |     }
290 | 
291 |     ////////////////////////////////////////////////////////////////////////////////
292 |     /// @brief The function that gets the data which might be received by GroupBulkRead::rxPacket or GroupBulkRead::txRxPacket
293 |     /// @param id Dynamixel ID
294 |     /// @param address Address of the data for read
295 |     /// @data_length Length of the data for read
296 |     /// @return data value
297 |     ////////////////////////////////////////////////////////////////////////////////
298 |     //   uint32_t    getData     (uint8_t id, uint16_t address, uint16_t data_length);
299 |     uint32_t getData(uint8_t id) {
300 |         //   if (isAvailable(id, address, data_length) == false)
301 |         //     return 0;
302 | 
303 |         //   uint16_t start_addr = address_list_[id];
304 | 
305 |         //   switch(data_length_)
306 |         switch (length_list_[id]) {
307 |             case 1:
308 |                 return data_list_[id][0];
309 | 
310 |             case 2:
311 |                 return DXL_MAKEWORD(data_list_[id][0], data_list_[id][1]);
312 | 
313 |             case 4:
314 |                 return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][0], data_list_[id][1]), DXL_MAKEWORD(data_list_[id][2], data_list_[id][3]));
315 | 
316 |             default:
317 |                 return 0;
318 |         }
319 |     }
320 | 
321 |     ////////////////////////////////////////////////////////////////////////////////
322 |     /// @brief The function that gets the error which might be received by GroupBulkRead::rxPacket or GroupBulkRead::txRxPacket
323 |     /// @param id Dynamixel ID
324 |     /// @error error of Dynamixel
325 |     /// @return true
326 |     /// @return   when Dynamixel returned specific error byte
327 |     /// @return or false
328 |     ////////////////////////////////////////////////////////////////////////////////
329 |     bool getError(uint8_t id, uint8_t *error) {
330 |         // TODO : check protocol version, last_result_, data_list
331 |         // if (last_result_ == false || error_list_.find(id) == error_list_.end())
332 | 
333 |         error[0] = error_list_[id][0];
334 | 
335 |         if (error[0] != 0) {
336 |             return true;
337 |         } else {
338 |             return false;
339 |         }
340 |     }
341 | 
342 |     ////////////////////////////////////////////////////////////////////////////////
343 |     /// @brief The function that transmits INST_BULK_READ instruction packet
344 |     /// @description The function makes an instruction packet with INST_BULK_READ,
345 |     /// @description transmits the packet with Protocol2PacketHandler::txPacket().
346 |     /// @param port PortHandler instance
347 |     /// @param param Parameter for Bulk Read {ID1, ADDR_L1, ADDR_H1, LEN_L1, LEN_H1, ID2, ADDR_L2, ADDR_H2, LEN_L2, LEN_H2, ...}
348 |     /// @param param_length Length of the data for Bulk Read
349 |     /// @return communication results which come from Protocol2PacketHandler::txPacket()
350 |     ////////////////////////////////////////////////////////////////////////////////
351 |     int bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length)
352 |     // BulkReadRx   -> GroupBulkRead class
353 |     // BulkReadTxRx -> GroupBulkRead class
354 |     {
355 |         int result = COMM_TX_FAIL;
356 | 
357 |         uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3));
358 |         // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H
359 | 
360 |         if (txpacket == NULL)
361 |             return result;
362 | 
363 |         txpacket[PKT_ID] = BROADCAST_ID;
364 |         txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3);  // 3: INST CRC16_L CRC16_H
365 |         txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3);  // 3: INST CRC16_L CRC16_H
366 |         txpacket[PKT_INSTRUCTION] = INST_BULK_READ;
367 | 
368 |         for (uint16_t s = 0; s < param_length; s++)
369 |             txpacket[PKT_PARAMETER0 + s] = param[s];
370 |         //memcpy(&txpacket[PKT_PARAMETER0], param, param_length);
371 | 
372 |         result = ph_->txPacket(txpacket);
373 |         if (result == COMM_SUCCESS) {
374 |             int wait_length = 0;
375 |             for (uint16_t i = 0; i < param_length; i += 5)
376 |                 wait_length += DXL_MAKEWORD(param[i + 3], param[i + 4]) + 10;
377 |             port->setPacketTimeout((uint16_t)wait_length);
378 |         }
379 | 
380 |         free(txpacket);
381 |         //delete[] txpacket;
382 |         return result;
383 |     }
384 | };
385 | 
386 | }  // namespace dynamixel
387 | 
388 | #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ */
389 | 


--------------------------------------------------------------------------------
/Dynamixel/lib/DynamixelSDK/include/group_bulk_write.h:
--------------------------------------------------------------------------------
  1 | // 2019.04.01 by Hideaki Tai : simplified to adapt only to Arduino
  2 | /*******************************************************************************
  3 | * Copyright 2017 ROBOTIS CO., LTD.
  4 | *
  5 | * Licensed under the Apache License, Version 2.0 (the "License");
  6 | * you may not use this file except in compliance with the License.
  7 | * You may obtain a copy of the License at
  8 | *
  9 | *     http://www.apache.org/licenses/LICENSE-2.0
 10 | *
 11 | * Unless required by applicable law or agreed to in writing, software
 12 | * distributed under the License is distributed on an "AS IS" BASIS,
 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 14 | * See the License for the specific language governing permissions and
 15 | * limitations under the License.
 16 | *******************************************************************************/
 17 | 
 18 | ////////////////////////////////////////////////////////////////////////////////
 19 | /// @file The file for Dynamixel Bulk Write
 20 | /// @author Zerom, Leon (RyuWoon Jung)
 21 | ////////////////////////////////////////////////////////////////////////////////
 22 | 
 23 | #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_
 24 | #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_
 25 | 
 26 | #include <ArxTypeTraits.h>
 27 | #include <ArxContainer.h>
 28 | #include "types.h"
 29 | // #include "port_handler.h"
 30 | #include "port_handler_arduino.h"
 31 | #include "packet_handler.h"
 32 | 
 33 | namespace dynamixel {
 34 | 
 35 | ////////////////////////////////////////////////////////////////////////////////
 36 | /// @brief The class for writing multiple Dynamixel data from different addresses with different lengths at once
 37 | ////////////////////////////////////////////////////////////////////////////////
 38 | class GroupBulkWrite {
 39 |     static const size_t TXPACKET_MAX_LEN = 1 * 1024;
 40 |     static const size_t RXPACKET_MAX_LEN = 1 * 1024;
 41 | 
 42 |     ///////////////// for Protocol 2.0 Packet /////////////////
 43 |     static const uint8_t PKT_HEADER0 = 0;
 44 |     static const uint8_t PKT_HEADER1 = 1;
 45 |     static const uint8_t PKT_HEADER2 = 2;
 46 |     static const uint8_t PKT_RESERVED = 3;
 47 |     static const uint8_t PKT_ID = 4;
 48 |     static const uint8_t PKT_LENGTH_L = 5;
 49 |     static const uint8_t PKT_LENGTH_H = 6;
 50 |     static const uint8_t PKT_INSTRUCTION = 7;
 51 |     static const uint8_t PKT_ERROR = 8;
 52 |     static const uint8_t PKT_PARAMETER0 = 8;
 53 | 
 54 |     ///////////////// Protocol 2.0 Error bit /////////////////
 55 |     static const uint8_t ERRNUM_RESULT_FAIL = 1;  // Failed to process the instruction packet.
 56 |     static const uint8_t ERRNUM_INSTRUCTION = 2;  // Instruction error
 57 |     static const uint8_t ERRNUM_CRC = 3;          // CRC check error
 58 |     static const uint8_t ERRNUM_DATA_RANGE = 4;   // Data range error
 59 |     static const uint8_t ERRNUM_DATA_LENGTH = 5;  // Data length error
 60 |     static const uint8_t ERRNUM_DATA_LIMIT = 6;   // Data limit error
 61 |     static const uint8_t ERRNUM_ACCESS = 7;       // Access error
 62 |     static const uint8_t ERRBIT_ALERT = 128;      //When the device has a problem, this bit is set to 1. Check "Device Status Check" value.
 63 | private:
 64 |     PortHandler *port_;
 65 |     PacketHandler *ph_;
 66 | 
 67 |     Vec<uint8_t> id_list_;
 68 |     Map<uint8_t, uint16_t> address_list_;  // <id, start_address>
 69 |     Map<uint8_t, uint16_t> length_list_;   // <id, data_length>
 70 |     Map<uint8_t, uint8_t *> data_list_;    // <id, data>
 71 | 
 72 |     bool is_param_changed_;
 73 | 
 74 |     uint8_t *param_;
 75 |     uint16_t param_length_;
 76 | 
 77 |     void makeParam() {
 78 |         if (param_ != 0)
 79 |             delete[] param_;
 80 |         param_ = 0;
 81 | 
 82 |         param_length_ = 0;
 83 |         for (unsigned int i = 0; i < id_list_.size(); i++)
 84 |             param_length_ += 1 + 2 + 2 + length_list_[id_list_[i]];
 85 | 
 86 |         param_ = new uint8_t[param_length_];
 87 | 
 88 |         int idx = 0;
 89 |         for (unsigned int i = 0; i < id_list_.size(); i++) {
 90 |             uint8_t id = id_list_[i];
 91 |             if (data_list_[id] == 0)
 92 |                 return;
 93 | 
 94 |             param_[idx++] = id;
 95 |             param_[idx++] = DXL_LOBYTE(address_list_[id]);
 96 |             param_[idx++] = DXL_HIBYTE(address_list_[id]);
 97 |             param_[idx++] = DXL_LOBYTE(length_list_[id]);
 98 |             param_[idx++] = DXL_HIBYTE(length_list_[id]);
 99 |             for (uint16_t c = 0; c < length_list_[id]; c++)
100 |                 param_[idx++] = (data_list_[id])[c];
101 |         }
102 |     }
103 | 
104 | public:
105 |     ////////////////////////////////////////////////////////////////////////////////
106 |     /// @brief The function that Initializes instance for Bulk Write
107 |     /// @param port PortHandler instance
108 |     /// @param ph PacketHandler instance
109 |     ////////////////////////////////////////////////////////////////////////////////
110 |     GroupBulkWrite(PortHandler *port, PacketHandler *ph)
111 |         : port_(port),
112 |           ph_(ph),
113 |           is_param_changed_(false),
114 |           param_(0),
115 |           param_length_(0) {
116 |         clearParam();
117 |     }
118 | 
119 |     ////////////////////////////////////////////////////////////////////////////////
120 |     /// @brief The function that calls clearParam function to clear the parameter list for Bulk Write
121 |     ////////////////////////////////////////////////////////////////////////////////
122 |     ~GroupBulkWrite() { clearParam(); }
123 | 
124 |     ////////////////////////////////////////////////////////////////////////////////
125 |     /// @brief The function that returns PortHandler instance
126 |     /// @return PortHandler instance
127 |     ////////////////////////////////////////////////////////////////////////////////
128 |     PortHandler *getPortHandler() { return port_; }
129 | 
130 |     ////////////////////////////////////////////////////////////////////////////////
131 |     /// @brief The function that returns PacketHandler instance
132 |     /// @return PacketHandler instance
133 |     ////////////////////////////////////////////////////////////////////////////////
134 |     PacketHandler *getPacketHandler() { return ph_; }
135 | 
136 |     ////////////////////////////////////////////////////////////////////////////////
137 |     /// @brief The function that adds id, start_address, data_length to the Bulk Write list
138 |     /// @param id Dynamixel ID
139 |     /// @param start_address Address of the data for write
140 |     /// @param data_length Length of the data for write
141 |     /// @return false
142 |     /// @return   when the ID exists already in the list
143 |     /// @return or true
144 |     ////////////////////////////////////////////////////////////////////////////////
145 |     bool addParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data) {
146 |         // if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end())   // id already exist
147 |         //   return false;
148 |         // if id is already exist, return false
149 |         for (const auto &i : id_list_)  // if id is already exist
150 |             if (i == id) return false;
151 | 
152 |         id_list_.push_back(id);
153 |         address_list_[id] = start_address;
154 |         length_list_[id] = data_length;
155 |         data_list_[id] = new uint8_t[data_length];
156 |         for (uint16_t c = 0; c < data_length; c++)
157 |             data_list_[id][c] = data[c];
158 | 
159 |         is_param_changed_ = true;
160 |         return true;
161 |     }
162 | 
163 |     ////////////////////////////////////////////////////////////////////////////////
164 |     /// @brief The function that removes id from the Bulk Write list
165 |     /// @param id Dynamixel ID
166 |     ////////////////////////////////////////////////////////////////////////////////
167 |     void removeParam(uint8_t id) {
168 |         // if id is NOT exist, return
169 |         Vec<uint8_t>::iterator it = id_list_.begin();
170 |         for (; it != id_list_.end(); ++it)
171 |             if (*it == id) break;
172 |         if (it == id_list_.end()) return;
173 | 
174 |         id_list_.erase(it);
175 |         address_list_.erase(id);
176 |         length_list_.erase(id);
177 |         delete[] data_list_[id];
178 |         data_list_.erase(id);
179 | 
180 |         is_param_changed_ = true;
181 |     }
182 | 
183 |     ////////////////////////////////////////////////////////////////////////////////
184 |     /// @brief The function that changes the data for write in id -> start_address -> data_length to the Bulk Write list
185 |     /// @param id Dynamixel ID
186 |     /// @param start_address Address of the data for write
187 |     /// @param data_length Length of the data for write
188 |     /// @param data for replacement
189 |     /// @return false
190 |     /// @return   when the ID doesn't exist in the list
191 |     /// @return or true
192 |     ////////////////////////////////////////////////////////////////////////////////
193 |     bool changeParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data) {
194 |         // if id is NOT exist, return false
195 |         Vec<uint8_t>::iterator it = id_list_.begin();
196 |         for (; it != id_list_.end(); ++it)
197 |             if (*it == id) break;
198 |         if (it == id_list_.end()) return false;
199 | 
200 |         address_list_[id] = start_address;
201 |         length_list_[id] = data_length;
202 |         delete[] data_list_[id];
203 |         data_list_[id] = new uint8_t[data_length];
204 |         for (uint16_t c = 0; c < data_length; c++)
205 |             data_list_[id][c] = data[c];
206 | 
207 |         is_param_changed_ = true;
208 |         return true;
209 |     }
210 | 
211 |     ////////////////////////////////////////////////////////////////////////////////
212 |     /// @brief The function that clears the Bulk Write list
213 |     ////////////////////////////////////////////////////////////////////////////////
214 |     void clearParam() {
215 |         for (unsigned int i = 0; i < id_list_.size(); i++)
216 |             delete[] data_list_[id_list_[i]];
217 | 
218 |         id_list_.clear();
219 |         address_list_.clear();
220 |         length_list_.clear();
221 |         data_list_.clear();
222 |         if (param_ != 0)
223 |             delete[] param_;
224 |         param_ = 0;
225 |     }
226 | 
227 |     ////////////////////////////////////////////////////////////////////////////////
228 |     /// @brief The function that transmits the Bulk Write instruction packet which might be constructed by GroupBulkWrite::addParam function
229 |     /// @return COMM_NOT_AVAILABLE
230 |     /// @return   when the list for Bulk Write is empty
231 |     /// @return   when Protocol1.0 has been used
232 |     /// @return or the other communication results which come from PacketHandler::bulkWriteTxOnly
233 |     ////////////////////////////////////////////////////////////////////////////////
234 |     int txPacket() {
235 |         if (is_param_changed_ == true || param_ == 0)
236 |             makeParam();
237 | 
238 |         return bulkWriteTxOnly(port_, param_, param_length_);
239 |     }
240 | 
241 |     ////////////////////////////////////////////////////////////////////////////////
242 |     /// @brief The function that transmits INST_BULK_WRITE instruction packet
243 |     /// @description The function makes an instruction packet with INST_BULK_WRITE,
244 |     /// @description transmits the packet with Protocol2PacketHandler::txRxPacket().
245 |     /// @param port PortHandler instance
246 |     /// @param param Parameter for Bulk Write {ID1, START_ADDR_L, START_ADDR_H, DATA_LEN_L, DATA_LEN_H, DATA0, DATA1, ..., DATAn, ID2, START_ADDR_L, START_ADDR_H, DATA_LEN_L, DATA_LEN_H, DATA0, DATA1, ..., DATAn}
247 |     /// @param param_length Length of the data for Bulk Write
248 |     /// @return communication results which come from Protocol2PacketHandler::txRxPacket()
249 |     ////////////////////////////////////////////////////////////////////////////////
250 |     int bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length) {
251 |         (void)port;
252 |         int result = COMM_TX_FAIL;
253 | 
254 |         uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3));
255 |         // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H
256 | 
257 |         if (txpacket == NULL)
258 |             return result;
259 | 
260 |         txpacket[PKT_ID] = BROADCAST_ID;
261 |         txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3);  // 3: INST CRC16_L CRC16_H
262 |         txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3);  // 3: INST CRC16_L CRC16_H
263 |         txpacket[PKT_INSTRUCTION] = INST_BULK_WRITE;
264 | 
265 |         for (uint16_t s = 0; s < param_length; s++)
266 |             txpacket[PKT_PARAMETER0 + s] = param[s];
267 |         //memcpy(&txpacket[PKT_PARAMETER0], param, param_length);
268 | 
269 |         result = ph_->txRxPacket(txpacket, 0, 0);
270 | 
271 |         free(txpacket);
272 |         //delete[] txpacket;
273 |         return result;
274 |     }
275 | };
276 | 
277 | }  // namespace dynamixel
278 | 
279 | #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ */
280 | 


--------------------------------------------------------------------------------
/Dynamixel/lib/DynamixelSDK/include/group_sync_read.h:
--------------------------------------------------------------------------------
  1 | // 2019.04.01 by Hideaki Tai : simplified to adapt only to Arduino
  2 | /*******************************************************************************
  3 | * Copyright 2017 ROBOTIS CO., LTD.
  4 | *
  5 | * Licensed under the Apache License, Version 2.0 (the "License");
  6 | * you may not use this file except in compliance with the License.
  7 | * You may obtain a copy of the License at
  8 | *
  9 | *     http://www.apache.org/licenses/LICENSE-2.0
 10 | *
 11 | * Unless required by applicable law or agreed to in writing, software
 12 | * distributed under the License is distributed on an "AS IS" BASIS,
 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 14 | * See the License for the specific language governing permissions and
 15 | * limitations under the License.
 16 | *******************************************************************************/
 17 | 
 18 | ////////////////////////////////////////////////////////////////////////////////
 19 | /// @file The file for Dynamixel Sync Read
 20 | /// @author Zerom, Leon (RyuWoon Jung)
 21 | ////////////////////////////////////////////////////////////////////////////////
 22 | 
 23 | #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_
 24 | #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_
 25 | 
 26 | #include <ArxTypeTraits.h>
 27 | #include <ArxContainer.h>
 28 | #include "types.h"
 29 | #include "port_handler_arduino.h"
 30 | #include "packet_handler.h"
 31 | 
 32 | namespace dynamixel {
 33 | 
 34 | ////////////////////////////////////////////////////////////////////////////////
 35 | /// @brief The class for reading multiple Dynamixel data from same address with same length at once
 36 | ////////////////////////////////////////////////////////////////////////////////
 37 | class GroupSyncRead {
 38 |     static const size_t TXPACKET_MAX_LEN = 1 * 1024;
 39 |     static const size_t RXPACKET_MAX_LEN = 1 * 1024;
 40 | 
 41 |     ///////////////// for Protocol 2.0 Packet /////////////////
 42 |     static const uint8_t PKT_HEADER0 = 0;
 43 |     static const uint8_t PKT_HEADER1 = 1;
 44 |     static const uint8_t PKT_HEADER2 = 2;
 45 |     static const uint8_t PKT_RESERVED = 3;
 46 |     static const uint8_t PKT_ID = 4;
 47 |     static const uint8_t PKT_LENGTH_L = 5;
 48 |     static const uint8_t PKT_LENGTH_H = 6;
 49 |     static const uint8_t PKT_INSTRUCTION = 7;
 50 |     static const uint8_t PKT_ERROR = 8;
 51 |     static const uint8_t PKT_PARAMETER0 = 8;
 52 | 
 53 |     ///////////////// Protocol 2.0 Error bit /////////////////
 54 |     static const uint8_t ERRNUM_RESULT_FAIL = 1;  // Failed to process the instruction packet.
 55 |     static const uint8_t ERRNUM_INSTRUCTION = 2;  // Instruction error
 56 |     static const uint8_t ERRNUM_CRC = 3;          // CRC check error
 57 |     static const uint8_t ERRNUM_DATA_RANGE = 4;   // Data range error
 58 |     static const uint8_t ERRNUM_DATA_LENGTH = 5;  // Data length error
 59 |     static const uint8_t ERRNUM_DATA_LIMIT = 6;   // Data limit error
 60 |     static const uint8_t ERRNUM_ACCESS = 7;       // Access error
 61 |     static const uint8_t ERRBIT_ALERT = 128;      //When the device has a problem, this bit is set to 1. Check "Device Status Check" value.
 62 | private:
 63 |     PortHandler *port_;
 64 |     PacketHandler *ph_;
 65 | 
 66 |     Vec<uint8_t> id_list_;
 67 |     Map<uint8_t, uint8_t *> data_list_;   // <id, data>
 68 |     Map<uint8_t, uint8_t *> error_list_;  // <id, error>
 69 | 
 70 |     bool last_result_;
 71 |     bool is_param_changed_;
 72 | 
 73 |     uint8_t *param_;
 74 |     uint16_t start_address_;
 75 |     uint16_t data_length_;
 76 | 
 77 |     void makeParam() {
 78 |         if (param_ != 0)
 79 |             delete[] param_;
 80 |         param_ = 0;
 81 | 
 82 |         param_ = new uint8_t[id_list_.size() * 1];  // ID(1)
 83 | 
 84 |         int idx = 0;
 85 |         for (unsigned int i = 0; i < id_list_.size(); i++)
 86 |             param_[idx++] = id_list_[i];
 87 |     }
 88 | 
 89 | public:
 90 |     void setAddress(uint16_t addr, uint16_t size) {
 91 |         start_address_ = addr;
 92 |         data_length_ = size;
 93 |     }
 94 | 
 95 |     ////////////////////////////////////////////////////////////////////////////////
 96 |     /// @brief The function that Initializes instance for Sync Read
 97 |     /// @param port PortHandler instance
 98 |     /// @param ph PacketHandler instance
 99 |     /// @param start_address Address of the data for read
100 |     /// @param data_length Length of the data for read
101 |     ////////////////////////////////////////////////////////////////////////////////
102 |     //   GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length);
103 |     GroupSyncRead(PortHandler *port, PacketHandler *ph)
104 |         : port_(port),
105 |           ph_(ph),
106 |           last_result_(false),
107 |           is_param_changed_(false),
108 |           param_(0)
109 |     // start_address_(start_address),
110 |     // data_length_(data_length)
111 |     {
112 |         clearParam();
113 |     }
114 | 
115 |     ////////////////////////////////////////////////////////////////////////////////
116 |     /// @brief The function that calls clearParam function to clear the parameter list for Sync Read
117 |     ////////////////////////////////////////////////////////////////////////////////
118 |     ~GroupSyncRead() { clearParam(); }
119 | 
120 |     ////////////////////////////////////////////////////////////////////////////////
121 |     /// @brief The function that returns PortHandler instance
122 |     /// @return PortHandler instance
123 |     ////////////////////////////////////////////////////////////////////////////////
124 |     PortHandler *getPortHandler() { return port_; }
125 | 
126 |     ////////////////////////////////////////////////////////////////////////////////
127 |     /// @brief The function that returns PacketHandler instance
128 |     /// @return PacketHandler instance
129 |     ////////////////////////////////////////////////////////////////////////////////
130 |     PacketHandler *getPacketHandler() { return ph_; }
131 | 
132 |     ////////////////////////////////////////////////////////////////////////////////
133 |     /// @brief The function that adds id, start_address, data_length to the Sync Read list
134 |     /// @param id Dynamixel ID
135 |     /// @return false
136 |     /// @return   when the ID exists already in the list
137 |     /// @return   when the protocol1.0 has been used
138 |     /// @return or true
139 |     ////////////////////////////////////////////////////////////////////////////////
140 |     bool addParam(uint8_t id) {
141 |         // if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end())   // id already exist
142 |         //   return false;
143 |         // if id is already exist, return false
144 |         for (const auto &i : id_list_)  // if id is already exist
145 |             if (i == id) return false;
146 | 
147 |         id_list_.push_back(id);
148 |         data_list_[id] = new uint8_t[data_length_];
149 |         error_list_[id] = new uint8_t[1];
150 | 
151 |         is_param_changed_ = true;
152 |         return true;
153 |     }
154 | 
155 |     ////////////////////////////////////////////////////////////////////////////////
156 |     /// @brief The function that removes id from the Sync Read list
157 |     /// @param id Dynamixel ID
158 |     ////////////////////////////////////////////////////////////////////////////////
159 |     void removeParam(uint8_t id) {
160 |         // if id is NOT exist, return
161 |         Vec<uint8_t>::iterator it = id_list_.begin();
162 |         for (; it != id_list_.end(); ++it)
163 |             if (*it == id) break;
164 |         if (it == id_list_.end()) return;
165 | 
166 |         id_list_.erase(it);
167 |         delete[] data_list_[id];
168 |         delete[] error_list_[id];
169 |         data_list_.erase(id);
170 |         error_list_.erase(id);
171 | 
172 |         is_param_changed_ = true;
173 |     }
174 | 
175 |     ////////////////////////////////////////////////////////////////////////////////
176 |     /// @brief The function that clears the Sync Read list
177 |     ////////////////////////////////////////////////////////////////////////////////
178 |     void clearParam() {
179 |         for (unsigned int i = 0; i < id_list_.size(); i++) {
180 |             delete[] data_list_[id_list_[i]];
181 |             delete[] error_list_[id_list_[i]];
182 |         }
183 | 
184 |         id_list_.clear();
185 |         data_list_.clear();
186 |         error_list_.clear();
187 |         if (param_ != 0)
188 |             delete[] param_;
189 |         param_ = 0;
190 |     }
191 | 
192 |     ////////////////////////////////////////////////////////////////////////////////
193 |     /// @brief The function that transmits the Sync Read instruction packet which might be constructed by GroupSyncRead::addParam function
194 |     /// @return COMM_NOT_AVAILABLE
195 |     /// @return   when the list for Sync Read is empty
196 |     /// @return   when the protocol1.0 has been used
197 |     /// @return or the other communication results which come from PacketHandler::syncReadTx
198 |     ////////////////////////////////////////////////////////////////////////////////
199 |     int txPacket() {
200 |         if (is_param_changed_ == true || param_ == 0)
201 |             makeParam();
202 | 
203 |         return syncReadTx(port_, start_address_, data_length_, param_, (uint16_t)id_list_.size() * 1);
204 |     }
205 | 
206 |     ////////////////////////////////////////////////////////////////////////////////
207 |     /// @brief The function that receives the packet which might be come from the Dynamixel
208 |     /// @return COMM_NOT_AVAILABLE
209 |     /// @return   when the list for Sync Read is empty
210 |     /// @return   when the protocol1.0 has been used
211 |     /// @return COMM_SUCCESS
212 |     /// @return   when there is packet recieved
213 |     /// @return or the other communication results
214 |     ////////////////////////////////////////////////////////////////////////////////
215 |     int rxPacket() {
216 |         last_result_ = false;
217 | 
218 |         int cnt = id_list_.size();
219 |         int result = COMM_RX_FAIL;
220 | 
221 |         if (cnt == 0)
222 |             return COMM_NOT_AVAILABLE;
223 | 
224 |         for (int i = 0; i < cnt; i++) {
225 |             uint8_t id = id_list_[i];
226 | 
227 |             result = ph_->readRx(id, data_length_, data_list_[id], error_list_[id]);
228 |             if (result != COMM_SUCCESS)
229 |                 return result;
230 |         }
231 | 
232 |         if (result == COMM_SUCCESS)
233 |             last_result_ = true;
234 | 
235 |         return result;
236 |     }
237 |     ////////////////////////////////////////////////////////////////////////////////
238 |     /// @brief The function that transmits and receives the packet which might be come from the Dynamixel
239 |     /// @return COMM_NOT_AVAILABLE
240 |     /// @return   when the protocol1.0 has been used
241 |     /// @return COMM_RX_FAIL
242 |     /// @return   when there is no packet recieved
243 |     /// @return COMM_SUCCESS
244 |     /// @return   when there is packet recieved
245 |     /// @return or the other communication results which come from GroupBulkRead::txPacket or GroupBulkRead::rxPacket
246 |     ////////////////////////////////////////////////////////////////////////////////
247 |     int txRxPacket() {
248 |         int result = COMM_TX_FAIL;
249 | 
250 |         result = txPacket();
251 |         if (result != COMM_SUCCESS)
252 |             return result;
253 | 
254 |         return rxPacket();
255 |     }
256 | 
257 |     ////////////////////////////////////////////////////////////////////////////////
258 |     /// @brief The function that checks whether there are available data which might be received by GroupSyncRead::rxPacket or GroupSyncRead::txRxPacket
259 |     /// @param id Dynamixel ID
260 |     /// @param address Address of the data for read
261 |     /// @param data_length Length of the data for read
262 |     /// @return false
263 |     /// @return   when there are no data available
264 |     /// @return   when the protocol1.0 has been used
265 |     /// @return or true
266 |     ////////////////////////////////////////////////////////////////////////////////
267 |     //   bool        isAvailable (uint8_t id, uint16_t address, uint16_t data_length);
268 |     bool isAvailable(uint8_t id) {
269 |         if (last_result_ == false || data_list_.find(id) == data_list_.end())
270 |             return false;
271 | 
272 |         // TODO: ?????
273 |         //   if (address < start_address_ || start_address_ + data_length_ - data_length < address)
274 |         //     return false;
275 | 
276 |         return true;
277 |     }
278 | 
279 |     ////////////////////////////////////////////////////////////////////////////////
280 |     /// @brief The function that gets the data which might be received by GroupSyncRead::rxPacket or GroupSyncRead::txRxPacket
281 |     /// @param id Dynamixel ID
282 |     /// @param address Address of the data for read
283 |     /// @data_length Length of the data for read
284 |     /// @return data value
285 |     ////////////////////////////////////////////////////////////////////////////////
286 |     //   uint32_t    getData     (uint8_t id, uint16_t address, uint16_t data_length);
287 |     uint32_t getData(uint8_t id) {
288 |         switch (data_length_) {
289 |             case 1:
290 |                 //   return data_list_[id][address - start_address_];
291 |                 return data_list_[id][0];
292 | 
293 |             case 2:
294 |                 //   return DXL_MAKEWORD(data_list_[id][address - start_address_], data_list_[id][address - start_address_ + 1]);
295 |                 return DXL_MAKEWORD(data_list_[id][0], data_list_[id][1]);
296 | 
297 |             case 4:
298 |                 //   return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_address_ + 0], data_list_[id][address - start_address_ + 1]),
299 |                 //              DXL_MAKEWORD(data_list_[id][address - start_address_ + 2], data_list_[id][address - start_address_ + 3]));
300 |                 return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][0], data_list_[id][1]), DXL_MAKEWORD(data_list_[id][2], data_list_[id][3]));
301 | 
302 |             default:
303 |                 return 0;
304 |         }
305 |     }
306 | 
307 |     ////////////////////////////////////////////////////////////////////////////////
308 |     /// @brief The function that gets the error which might be received by GroupSyncRead::rxPacket or GroupSyncRead::txRxPacket
309 |     /// @param id Dynamixel ID
310 |     /// @error error of Dynamixel
311 |     /// @return true
312 |     /// @return   when Dynamixel returned specific error byte
313 |     /// @return or false
314 |     ////////////////////////////////////////////////////////////////////////////////
315 |     bool getError(uint8_t id, uint8_t *error) {
316 |         // TODO : check protocol version, last_result_, data_list
317 |         // if (last_result_ == false || error_list_.find(id) == error_list_.end())
318 | 
319 |         error[0] = error_list_[id][0];
320 | 
321 |         if (error[0] != 0) {
322 |             return true;
323 |         } else {
324 |             return false;
325 |         }
326 |     }
327 | 
328 |     ////////////////////////////////////////////////////////////////////////////////
329 |     /// @brief The function that transmits INST_SYNC_READ instruction packet
330 |     /// @description The function makes an instruction packet with INST_SYNC_READ,
331 |     /// @description transmits the packet with Protocol2PacketHandler::txPacket().
332 |     /// @param port PortHandler instance
333 |     /// @param start_address Address of the data for Sync Read
334 |     /// @param data_length Length of the data for Sync Read
335 |     /// @param param Parameter for Sync Read
336 |     /// @param param_length Length of the data for Sync Read
337 |     /// @return communication results which come from Protocol2PacketHandler::txPacket()
338 |     ////////////////////////////////////////////////////////////////////////////////
339 |     int syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length)
340 |     // SyncReadRx   -> GroupSyncRead class
341 |     // SyncReadTxRx -> GroupSyncRead class
342 |     {
343 |         int result = COMM_TX_FAIL;
344 | 
345 |         uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3));
346 |         // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H
347 | 
348 |         if (txpacket == NULL)
349 |             return result;
350 | 
351 |         txpacket[PKT_ID] = BROADCAST_ID;
352 |         txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7);  // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H
353 |         txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7);  // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H
354 |         txpacket[PKT_INSTRUCTION] = INST_SYNC_READ;
355 |         txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(start_address);
356 |         txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(start_address);
357 |         txpacket[PKT_PARAMETER0 + 2] = DXL_LOBYTE(data_length);
358 |         txpacket[PKT_PARAMETER0 + 3] = DXL_HIBYTE(data_length);
359 | 
360 |         for (uint16_t s = 0; s < param_length; s++)
361 |             txpacket[PKT_PARAMETER0 + 4 + s] = param[s];
362 |         //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length);
363 | 
364 |         result = ph_->txPacket(txpacket);
365 |         if (result == COMM_SUCCESS)
366 |             port->setPacketTimeout((uint16_t)((11 + data_length) * param_length));
367 | 
368 |         free(txpacket);
369 |         return result;
370 |     }
371 | };
372 | 
373 | }  // namespace dynamixel
374 | 
375 | #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ */
376 | 


--------------------------------------------------------------------------------
/Dynamixel/lib/DynamixelSDK/include/group_sync_write.h:
--------------------------------------------------------------------------------
  1 | // 2019.04.01 by Hideaki Tai : simplified to adapt only to Arduino
  2 | /*******************************************************************************
  3 | * Copyright 2017 ROBOTIS CO., LTD.
  4 | *
  5 | * Licensed under the Apache License, Version 2.0 (the "License");
  6 | * you may not use this file except in compliance with the License.
  7 | * You may obtain a copy of the License at
  8 | *
  9 | *     http://www.apache.org/licenses/LICENSE-2.0
 10 | *
 11 | * Unless required by applicable law or agreed to in writing, software
 12 | * distributed under the License is distributed on an "AS IS" BASIS,
 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 14 | * See the License for the specific language governing permissions and
 15 | * limitations under the License.
 16 | *******************************************************************************/
 17 | 
 18 | ////////////////////////////////////////////////////////////////////////////////
 19 | /// @file The file for Dynamixel Sync Write
 20 | /// @author Zerom, Leon (RyuWoon Jung)
 21 | ////////////////////////////////////////////////////////////////////////////////
 22 | 
 23 | #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_
 24 | #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_
 25 | 
 26 | #include <ArxTypeTraits.h>
 27 | #include <ArxContainer.h>
 28 | #include "types.h"
 29 | // #include "port_handler.h"
 30 | #include "port_handler_arduino.h"
 31 | #include "packet_handler.h"
 32 | 
 33 | namespace dynamixel {
 34 | 
 35 | ////////////////////////////////////////////////////////////////////////////////
 36 | /// @brief The class for writing multiple Dynamixel data from same address with same length at once
 37 | ////////////////////////////////////////////////////////////////////////////////
 38 | class GroupSyncWrite {
 39 |     static const size_t TXPACKET_MAX_LEN = 1 * 1024;
 40 |     static const size_t RXPACKET_MAX_LEN = 1 * 1024;
 41 | 
 42 |     ///////////////// for Protocol 2.0 Packet /////////////////
 43 |     static const uint8_t PKT_HEADER0 = 0;
 44 |     static const uint8_t PKT_HEADER1 = 1;
 45 |     static const uint8_t PKT_HEADER2 = 2;
 46 |     static const uint8_t PKT_RESERVED = 3;
 47 |     static const uint8_t PKT_ID = 4;
 48 |     static const uint8_t PKT_LENGTH_L = 5;
 49 |     static const uint8_t PKT_LENGTH_H = 6;
 50 |     static const uint8_t PKT_INSTRUCTION = 7;
 51 |     static const uint8_t PKT_ERROR = 8;
 52 |     static const uint8_t PKT_PARAMETER0 = 8;
 53 | 
 54 |     ///////////////// Protocol 2.0 Error bit /////////////////
 55 |     static const uint8_t ERRNUM_RESULT_FAIL = 1;  // Failed to process the instruction packet.
 56 |     static const uint8_t ERRNUM_INSTRUCTION = 2;  // Instruction error
 57 |     static const uint8_t ERRNUM_CRC = 3;          // CRC check error
 58 |     static const uint8_t ERRNUM_DATA_RANGE = 4;   // Data range error
 59 |     static const uint8_t ERRNUM_DATA_LENGTH = 5;  // Data length error
 60 |     static const uint8_t ERRNUM_DATA_LIMIT = 6;   // Data limit error
 61 |     static const uint8_t ERRNUM_ACCESS = 7;       // Access error
 62 |     static const uint8_t ERRBIT_ALERT = 128;      //When the device has a problem, this bit is set to 1. Check "Device Status Check" value.
 63 | private:
 64 |     PortHandler *port_;
 65 |     PacketHandler *ph_;
 66 | 
 67 |     Vec<uint8_t> id_list_;
 68 |     Map<uint8_t, uint8_t *> data_list_;  // <id, data>
 69 | 
 70 |     bool is_param_changed_;
 71 | 
 72 |     uint8_t *param_;
 73 |     uint16_t start_address_;
 74 |     uint16_t data_length_;
 75 | 
 76 |     void makeParam() {
 77 |         if (id_list_.size() == 0) return;
 78 | 
 79 |         if (param_ != 0)
 80 |             delete[] param_;
 81 |         param_ = 0;
 82 | 
 83 |         param_ = new uint8_t[id_list_.size() * (1 + data_length_)];  // ID(1) + DATA(data_length)
 84 | 
 85 |         int idx = 0;
 86 |         for (unsigned int i = 0; i < id_list_.size(); i++) {
 87 |             uint8_t id = id_list_[i];
 88 |             if (data_list_[id] == 0)
 89 |                 return;
 90 | 
 91 |             param_[idx++] = id;
 92 |             for (uint16_t c = 0; c < data_length_; c++)
 93 |                 param_[idx++] = (data_list_[id])[c];
 94 |         }
 95 |     }
 96 | 
 97 | public:
 98 |     void setAddress(uint16_t addr, uint16_t size) {
 99 |         start_address_ = addr;
100 |         data_length_ = size;
101 |     }
102 | 
103 |     ////////////////////////////////////////////////////////////////////////////////
104 |     /// @brief The function that Initializes instance for Sync Write
105 |     /// @param port PortHandler instance
106 |     /// @param ph PacketHandler instance
107 |     /// @param start_address Address of the data for write
108 |     /// @param data_length Length of the data for write
109 |     ////////////////////////////////////////////////////////////////////////////////
110 |     //   GroupSyncWrite(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length);
111 |     GroupSyncWrite(PortHandler *port, PacketHandler *ph)
112 |         : port_(port),
113 |           ph_(ph),
114 |           is_param_changed_(false),
115 |           param_(0)
116 |     // start_address_(start_address),
117 |     // data_length_(data_length)
118 |     {
119 |         clearParam();
120 |     }
121 | 
122 |     ////////////////////////////////////////////////////////////////////////////////
123 |     /// @brief The function that calls clearParam function to clear the parameter list for Sync Write
124 |     ////////////////////////////////////////////////////////////////////////////////
125 |     ~GroupSyncWrite() { clearParam(); }
126 | 
127 |     ////////////////////////////////////////////////////////////////////////////////
128 |     /// @brief The function that returns PortHandler instance
129 |     /// @return PortHandler instance
130 |     ////////////////////////////////////////////////////////////////////////////////
131 |     PortHandler *getPortHandler() { return port_; }
132 | 
133 |     ////////////////////////////////////////////////////////////////////////////////
134 |     /// @brief The function that returns PacketHandler instance
135 |     /// @return PacketHandler instance
136 |     ////////////////////////////////////////////////////////////////////////////////
137 |     PacketHandler *getPacketHandler() { return ph_; }
138 | 
139 |     ////////////////////////////////////////////////////////////////////////////////
140 |     /// @brief The function that adds id, start_address, data_length to the Sync Write list
141 |     /// @param id Dynamixel ID
142 |     /// @param data Data for write
143 |     /// @return false
144 |     /// @return   when the ID exists already in the list
145 |     /// @return or true
146 |     ////////////////////////////////////////////////////////////////////////////////
147 |     bool addParam(uint8_t id, uint8_t *data) {
148 |         // if id is already exist, return false
149 |         for (const auto &i : id_list_)  // if id is already exist
150 |             if (i == id) return false;
151 | 
152 |         id_list_.push_back(id);
153 |         data_list_[id] = new uint8_t[data_length_];
154 |         for (uint16_t c = 0; c < data_length_; c++)
155 |             data_list_[id][c] = data[c];
156 | 
157 |         is_param_changed_ = true;
158 |         return true;
159 |     }
160 | 
161 |     ////////////////////////////////////////////////////////////////////////////////
162 |     /// @brief The function that removes id from the Sync Write list
163 |     /// @param id Dynamixel ID
164 |     ////////////////////////////////////////////////////////////////////////////////
165 |     void removeParam(uint8_t id) {
166 |         // if id is NOT exist, return
167 |         Vec<uint8_t>::iterator it = id_list_.begin();
168 |         for (; it != id_list_.end(); ++it)
169 |             if (*it == id) break;
170 |         if (it == id_list_.end()) return;
171 | 
172 |         id_list_.erase(it);
173 |         delete[] data_list_[id];
174 |         data_list_.erase(id);
175 | 
176 |         is_param_changed_ = true;
177 |     }
178 | 
179 |     ////////////////////////////////////////////////////////////////////////////////
180 |     /// @brief The function that changes the data for write in id -> start_address -> data_length to the Sync Write list
181 |     /// @param id Dynamixel ID
182 |     /// @param data for replacement
183 |     /// @return false
184 |     /// @return   when the ID doesn't exist in the list
185 |     /// @return or true
186 |     ////////////////////////////////////////////////////////////////////////////////
187 |     bool changeParam(uint8_t id, uint8_t *data) {
188 |         // if id is NOT exist, return false
189 |         Vec<uint8_t>::iterator it = id_list_.begin();
190 |         for (; it != id_list_.end(); ++it)
191 |             if (*it == id) break;
192 |         if (it == id_list_.end()) return false;
193 | 
194 |         delete[] data_list_[id];
195 |         data_list_[id] = new uint8_t[data_length_];
196 |         for (uint16_t c = 0; c < data_length_; c++)
197 |             data_list_[id][c] = data[c];
198 | 
199 |         is_param_changed_ = true;
200 |         return true;
201 |     }
202 | 
203 |     ////////////////////////////////////////////////////////////////////////////////
204 |     /// @brief The function that clears the Sync Write list
205 |     ////////////////////////////////////////////////////////////////////////////////
206 |     void clearParam() {
207 |         if (id_list_.size() == 0)
208 |             return;
209 | 
210 |         for (unsigned int i = 0; i < id_list_.size(); i++)
211 |             delete[] data_list_[id_list_[i]];
212 | 
213 |         id_list_.clear();
214 |         data_list_.clear();
215 |         if (param_ != 0)
216 |             delete[] param_;
217 |         param_ = 0;
218 |     }
219 | 
220 |     ////////////////////////////////////////////////////////////////////////////////
221 |     /// @brief The function that transmits the Sync Write instruction packet which might be constructed by GroupSyncWrite::addParam function
222 |     /// @return COMM_NOT_AVAILABLE
223 |     /// @return   when the list for Sync Write is empty
224 |     /// @return or the other communication results which come from PacketHandler::syncWriteTxOnly
225 |     ////////////////////////////////////////////////////////////////////////////////
226 |     int txPacket() {
227 |         if (id_list_.size() == 0)
228 |             return COMM_NOT_AVAILABLE;
229 | 
230 |         if (is_param_changed_ == true || param_ == 0)
231 |             makeParam();
232 | 
233 |         return syncWriteTxOnly(port_, start_address_, data_length_, param_, id_list_.size() * (1 + data_length_));
234 |     }
235 | 
236 |     ////////////////////////////////////////////////////////////////////////////////
237 |     /// @brief The function that transmits INST_SYNC_WRITE instruction packet
238 |     /// @description The function makes an instruction packet with INST_SYNC_WRITE,
239 |     /// @description transmits the packet with Protocol2PacketHandler::txRxPacket().
240 |     /// @param port PortHandler instance
241 |     /// @param start_address Address of the data for Sync Write
242 |     /// @param data_length Length of the data for Sync Write
243 |     /// @param param Parameter for Sync Write {ID1, DATA0, DATA1, ..., DATAn, ID2, DATA0, DATA1, ..., DATAn, ID3, DATA0, DATA1, ..., DATAn}
244 |     /// @param param_length Length of the data for Sync Write
245 |     /// @return communication results which come from Protocol2PacketHandler::txRxPacket()
246 |     ////////////////////////////////////////////////////////////////////////////////
247 |     int syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) {
248 |         (void)port;
249 |         int result = COMM_TX_FAIL;
250 | 
251 |         uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3));
252 |         // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H
253 | 
254 |         if (txpacket == NULL)
255 |             return result;
256 | 
257 |         txpacket[PKT_ID] = BROADCAST_ID;
258 |         txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7);  // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H
259 |         txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7);  // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H
260 |         txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE;
261 |         txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(start_address);
262 |         txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(start_address);
263 |         txpacket[PKT_PARAMETER0 + 2] = DXL_LOBYTE(data_length);
264 |         txpacket[PKT_PARAMETER0 + 3] = DXL_HIBYTE(data_length);
265 | 
266 |         for (uint16_t s = 0; s < param_length; s++)
267 |             txpacket[PKT_PARAMETER0 + 4 + s] = param[s];
268 |         //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length);
269 | 
270 |         result = ph_->txRxPacket(txpacket, 0, 0);
271 | 
272 |         free(txpacket);
273 |         //delete[] txpacket;
274 |         return result;
275 |     }
276 | };
277 | 
278 | }  // namespace dynamixel
279 | 
280 | #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ */
281 | 


--------------------------------------------------------------------------------
/Dynamixel/lib/DynamixelSDK/include/packet_handler.h:
--------------------------------------------------------------------------------
  1 | // 2019.04.01 by Hideaki Tai : simplified to adapt only to Arduino
  2 | /*******************************************************************************
  3 | * Copyright 2017 ROBOTIS CO., LTD.
  4 | *
  5 | * Licensed under the Apache License, Version 2.0 (the "License");
  6 | * you may not use this file except in compliance with the License.
  7 | * You may obtain a copy of the License at
  8 | *
  9 | *     http://www.apache.org/licenses/LICENSE-2.0
 10 | *
 11 | * Unless required by applicable law or agreed to in writing, software
 12 | * distributed under the License is distributed on an "AS IS" BASIS,
 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 14 | * See the License for the specific language governing permissions and
 15 | * limitations under the License.
 16 | *******************************************************************************/
 17 | 
 18 | ////////////////////////////////////////////////////////////////////////////////
 19 | /// @file The file for Dynamixel packet control
 20 | /// @author Zerom, Leon (RyuWoon Jung)
 21 | ////////////////////////////////////////////////////////////////////////////////
 22 | 
 23 | #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_
 24 | #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_
 25 | 
 26 | #include <Arduino.h>
 27 | #include <ArxTypeTraits.h>
 28 | #include <ArxContainer.h>
 29 | #include "types.h"
 30 | #include "port_handler_arduino.h"
 31 | 
 32 | #define BROADCAST_ID 0xFE  // 254
 33 | #define MAX_ID 0xFC        // 252
 34 | 
 35 | /* Macro for Control Table Value */
 36 | #define DXL_MAKEWORD(a, b) ((uint16_t)(((uint8_t)(((uint64_t)(a)) & 0xff)) | ((uint16_t)((uint8_t)(((uint64_t)(b)) & 0xff))) << 8))
 37 | #define DXL_MAKEDWORD(a, b) ((uint32_t)(((uint16_t)(((uint64_t)(a)) & 0xffff)) | ((uint32_t)((uint16_t)(((uint64_t)(b)) & 0xffff))) << 16))
 38 | #define DXL_LOWORD(l) ((uint16_t)(((uint64_t)(l)) & 0xffff))
 39 | #define DXL_HIWORD(l) ((uint16_t)((((uint64_t)(l)) >> 16) & 0xffff))
 40 | #define DXL_LOBYTE(w) ((uint8_t)(((uint64_t)(w)) & 0xff))
 41 | #define DXL_HIBYTE(w) ((uint8_t)((((uint64_t)(w)) >> 8) & 0xff))
 42 | 
 43 | /* Instruction for DXL Protocol */
 44 | #define INST_PING 1
 45 | #define INST_READ 2
 46 | #define INST_WRITE 3
 47 | #define INST_REG_WRITE 4
 48 | #define INST_ACTION 5
 49 | #define INST_FACTORY_RESET 6
 50 | #define INST_SYNC_WRITE 131  // 0x83
 51 | #define INST_BULK_READ 146   // 0x92
 52 | // --- Only for 2.0 --- //
 53 | #define INST_REBOOT 8
 54 | #define INST_CLEAR 16        // 0x10
 55 | #define INST_STATUS 85       // 0x55
 56 | #define INST_SYNC_READ 130   // 0x82
 57 | #define INST_BULK_WRITE 147  // 0x93
 58 | 
 59 | // Communication Result
 60 | #define COMM_SUCCESS 0            // tx or rx packet communication success
 61 | #define COMM_PORT_BUSY -1000      // Port is busy (in use)
 62 | #define COMM_TX_FAIL -1001        // Failed transmit instruction packet
 63 | #define COMM_RX_FAIL -1002        // Failed get status packet
 64 | #define COMM_TX_ERROR -2000       // Incorrect instruction packet
 65 | #define COMM_RX_WAITING -3000     // Now recieving status packet
 66 | #define COMM_RX_TIMEOUT -3001     // There is no status packet
 67 | #define COMM_RX_CORRUPT -3002     // Incorrect status packet
 68 | #define COMM_NOT_AVAILABLE -9000  //
 69 | 
 70 | namespace dynamixel {
 71 | 
 72 | class PacketHandler {
 73 | public:
 74 |     PortHandler *port;
 75 | 
 76 |     virtual ~PacketHandler() {}
 77 | 
 78 |     void attach(PortHandler *p) { port = p; }
 79 | 
 80 |     virtual float getProtocolVersion() = 0;
 81 | 
 82 |     const char *getTxRxResult(int result) {
 83 | #ifndef __AVR__
 84 |         switch (result) {
 85 |             case COMM_SUCCESS:
 86 |                 return "[TxRxResult] Communication success.";
 87 |             case COMM_PORT_BUSY:
 88 |                 return "[TxRxResult] Port is in use!";
 89 |             case COMM_TX_FAIL:
 90 |                 return "[TxRxResult] Failed transmit instruction packet!";
 91 |             case COMM_RX_FAIL:
 92 |                 return "[TxRxResult] Failed get status packet from device!";
 93 |             case COMM_TX_ERROR:
 94 |                 return "[TxRxResult] Incorrect instruction packet!";
 95 |             case COMM_RX_WAITING:
 96 |                 return "[TxRxResult] Now recieving status packet!";
 97 |             case COMM_RX_TIMEOUT:
 98 |                 return "[TxRxResult] There is no status packet!";
 99 |             case COMM_RX_CORRUPT:
100 |                 return "[TxRxResult] Incorrect status packet!";
101 |             case COMM_NOT_AVAILABLE:
102 |                 return "[TxRxResult] Protocol does not support This function!";
103 |             default:
104 |                 return "";
105 |         }
106 | #else
107 |         (void)result;
108 |         return "";
109 | #endif
110 |     }
111 | 
112 |     virtual const char *getRxPacketError(uint8_t error) = 0;
113 | 
114 |     virtual int ping(uint8_t id, uint16_t *model_number, uint8_t *error = 0) = 0;
115 |     virtual int broadcastPing(Vec<uint8_t> &id_list) = 0;
116 |     virtual int reboot(uint8_t id, uint8_t *error = 0) = 0;
117 |     virtual int clearMultiTurn(uint8_t id, uint8_t *error = 0) = 0;
118 |     virtual int factoryReset(uint8_t id, uint8_t option = 0, uint8_t *error = 0) = 0;
119 | 
120 |     int readBytesTxRx(uint8_t id, uint16_t address, uint8_t *data, uint8_t size, uint8_t *error = 0) {
121 |         uint8_t data_read[size];
122 |         int result = readTxRx(id, address, size, data_read, error);
123 |         if (result == COMM_SUCCESS)
124 |             for (uint8_t i = 0; i < size; ++i) data[i] = data_read[i];
125 |         return result;
126 |     }
127 | 
128 |     template <typename T, typename std::enable_if<std::is_integral<T>::value>::type * = nullptr>
129 |     int writeBytesTxRx(uint8_t id, uint16_t address, T data, uint8_t size, uint8_t *error = 0) {
130 |         return writeTxRx(id, address, size, (uint8_t *)&data, error);
131 |     }
132 | 
133 |     virtual int regWriteTxRx(uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0;
134 |     virtual int action(uint8_t id) = 0;
135 | 
136 |     virtual int readTxRx(uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0;
137 |     virtual int txPacket(uint8_t *txpacket) = 0;
138 |     virtual int rxPacket(uint8_t *rxpacket) = 0;
139 |     virtual int txRxPacket(uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0) = 0;
140 |     virtual int writeTxRx(uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0;
141 | 
142 |     // only for sync_read, bulk_read
143 |     virtual int readRx(uint8_t id, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0;
144 | 
145 | private:
146 | };
147 | 
148 | }  // namespace dynamixel
149 | 
150 | #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ */
151 | 


--------------------------------------------------------------------------------
/Dynamixel/lib/DynamixelSDK/include/port_handler_arduino.h:
--------------------------------------------------------------------------------
  1 | // 2019.04.01 by Hideaki Tai : simplified to adapt only to Arduino
  2 | /*******************************************************************************
  3 | * Copyright 2017 ROBOTIS CO., LTD.
  4 | *
  5 | * Licensed under the Apache License, Version 2.0 (the "License");
  6 | * you may not use this file except in compliance with the License.
  7 | * You may obtain a copy of the License at
  8 | *
  9 | *     http://www.apache.org/licenses/LICENSE-2.0
 10 | *
 11 | * Unless required by applicable law or agreed to in writing, software
 12 | * distributed under the License is distributed on an "AS IS" BASIS,
 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 14 | * See the License for the specific language governing permissions and
 15 | * limitations under the License.
 16 | *******************************************************************************/
 17 | 
 18 | ////////////////////////////////////////////////////////////////////////////////
 19 | /// @file The file for port control in Arduino
 20 | /// @author Cho (Hancheol Cho), Leon (RyuWoon Jung)
 21 | ////////////////////////////////////////////////////////////////////////////////
 22 | 
 23 | #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ARDUINO_PORTHANDLERARDUINO_H_
 24 | #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ARDUINO_PORTHANDLERARDUINO_H_
 25 | 
 26 | #include <Arduino.h>
 27 | #include "types.h"
 28 | 
 29 | namespace dynamixel {
 30 | 
 31 | class PortHandler {
 32 | private:
 33 |     const uint8_t pin_rts_enable;
 34 |     const uint8_t pin_rx_enable;
 35 |     const uint8_t pin_tx_enable;
 36 |     const size_t LATENCY_TIMER = 4;
 37 |     const size_t DEFAULT_BAUDRATE = 57600;
 38 | 
 39 |     int baudrate_;
 40 |     double packet_start_time_;
 41 |     double packet_timeout_;
 42 |     double tx_time_per_byte;
 43 | 
 44 |     Stream* stream;
 45 | 
 46 | public:
 47 |     PortHandler(uint8_t pin_rts_enable)
 48 |         : pin_rts_enable(pin_rts_enable), pin_rx_enable(0xFF), pin_tx_enable(0xFF), baudrate_(DEFAULT_BAUDRATE), packet_start_time_(0.0), packet_timeout_(0.0), tx_time_per_byte(0.0) {
 49 |         pinMode(pin_rts_enable, OUTPUT);
 50 |         // TODO: LOW-ACTIVE, HIGH-ACTIVE
 51 |         digitalWrite(pin_rts_enable, LOW);
 52 |     }
 53 |     PortHandler(uint8_t pin_rx_enable, uint8_t pin_tx_enable)
 54 |         : pin_rts_enable(0xFF), pin_rx_enable(pin_rx_enable), pin_tx_enable(pin_tx_enable), baudrate_(DEFAULT_BAUDRATE), packet_start_time_(0.0), packet_timeout_(0.0), tx_time_per_byte(0.0) {
 55 |         pinMode(pin_rx_enable, OUTPUT);
 56 |         pinMode(pin_tx_enable, OUTPUT);
 57 |         // TODO: LOW-ACTIVE, HIGH-ACTIVE
 58 |         digitalWrite(pin_rx_enable, LOW);
 59 |         digitalWrite(pin_tx_enable, LOW);
 60 |     }
 61 | 
 62 |     void attach(Stream& s, size_t baud) {
 63 |         stream = &s;
 64 |         baudrate_ = baud;
 65 |         tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0;
 66 |         set_tx(false);
 67 |     }
 68 | 
 69 |     void clearPort() {
 70 |         stream->flush();
 71 |         while (stream->available()) stream->read();
 72 |     }
 73 | 
 74 |     int getBytesAvailable() { return stream->available(); }
 75 | 
 76 |     int readPort(uint8_t* packet, int length) {
 77 |         int rx_length = stream->available();
 78 |         if (rx_length > length) rx_length = length;
 79 | 
 80 |         for (int i = 0; i < rx_length; i++) packet[i] = stream->read();
 81 |         return rx_length;
 82 |     }
 83 | 
 84 |     int writePort(uint8_t* packet, int length) {
 85 |         set_tx(true);
 86 |         int length_written = stream->write(packet, length);
 87 |         set_tx(false);
 88 |         return length_written;
 89 |     }
 90 | 
 91 |     void setPacketTimeout(uint16_t packet_length) {
 92 |         packet_start_time_ = getCurrentTime();
 93 |         packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0;
 94 |     }
 95 | 
 96 |     void setPacketTimeout(double msec) {
 97 |         packet_start_time_ = getCurrentTime();
 98 |         packet_timeout_ = msec;
 99 |     }
100 | 
101 |     bool isPacketTimeout() {
102 |         if (getTimeSinceStart() > packet_timeout_) {
103 |             packet_timeout_ = 0;
104 |             return true;
105 |         }
106 |         return false;
107 |     }
108 | 
109 |     double getCurrentTime() {
110 |         return (double)millis();
111 |     }
112 | 
113 |     double getTimeSinceStart() {
114 |         double elapsed_time;
115 | 
116 |         elapsed_time = getCurrentTime() - packet_start_time_;
117 |         if (elapsed_time < 0.0) packet_start_time_ = getCurrentTime();
118 |         return elapsed_time;
119 |     }
120 | 
121 | private:
122 |     inline bool isOneRtsPin() { return !(pin_rts_enable == 0xFF); }
123 | 
124 |     void set_tx(bool b) {
125 |         if (!b) stream->flush();  // make sure it completes before we disable...
126 |         drv_dxl_tx_enable(b);
127 |     }
128 | 
129 |     void drv_dxl_tx_enable(bool enable) {
130 |         // TODO: LOW-ACTIVE, HIGH-ACTIVE
131 |         if (isOneRtsPin()) {
132 |             if (enable)
133 |                 digitalWrite(pin_rts_enable, HIGH);
134 |             else
135 |                 digitalWrite(pin_rts_enable, LOW);
136 |         } else {
137 |             if (enable) {
138 |                 digitalWrite(pin_rx_enable, LOW);
139 |                 digitalWrite(pin_tx_enable, HIGH);
140 |             } else {
141 |                 digitalWrite(pin_rx_enable, HIGH);
142 |                 digitalWrite(pin_tx_enable, LOW);
143 |             }
144 |         }
145 |     }
146 | };
147 | 
148 | }  // namespace dynamixel
149 | 
150 | #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ARDUINO_PORTHANDLERARDUINO_H_ */
151 | 


--------------------------------------------------------------------------------
/Dynamixel/lib/DynamixelSDK/include/protocol1_packet_handler.h:
--------------------------------------------------------------------------------
  1 | // 2019.04.01 by Hideaki Tai : simplified to adapt only to Arduino
  2 | /*******************************************************************************
  3 | * Copyright 2017 ROBOTIS CO., LTD.
  4 | *
  5 | * Licensed under the Apache License, Version 2.0 (the "License");
  6 | * you may not use this file except in compliance with the License.
  7 | * You may obtain a copy of the License at
  8 | *
  9 | *     http://www.apache.org/licenses/LICENSE-2.0
 10 | *
 11 | * Unless required by applicable law or agreed to in writing, software
 12 | * distributed under the License is distributed on an "AS IS" BASIS,
 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 14 | * See the License for the specific language governing permissions and
 15 | * limitations under the License.
 16 | *******************************************************************************/
 17 | 
 18 | ////////////////////////////////////////////////////////////////////////////////
 19 | /// @file The file for Protocol 1.0 Dynamixel packet control
 20 | /// @author Zerom, Leon (RyuWoon Jung)
 21 | ////////////////////////////////////////////////////////////////////////////////
 22 | 
 23 | #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_
 24 | #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_
 25 | 
 26 | #include "types.h"
 27 | #include "packet_handler.h"
 28 | 
 29 | namespace dynamixel {
 30 | 
 31 | class Protocol1PacketHandler : public PacketHandler {
 32 |     static const size_t TXPACKET_MAX_LEN = 250;
 33 |     static const size_t RXPACKET_MAX_LEN = 250;
 34 | 
 35 |     ///////////////// for Protocol 1.0 Packet /////////////////
 36 |     static const uint8_t PKT_HEADER0 = 0;
 37 |     static const uint8_t PKT_HEADER1 = 1;
 38 |     static const uint8_t PKT_ID = 2;
 39 |     static const uint8_t PKT_LENGTH = 3;
 40 |     static const uint8_t PKT_INSTRUCTION = 4;
 41 |     static const uint8_t PKT_ERROR = 4;
 42 |     static const uint8_t PKT_PARAMETER0 = 5;
 43 | 
 44 |     ///////////////// Protocol 1.0 Error bit /////////////////
 45 |     static const uint8_t ERRBIT_VOLTAGE = 1;       // Supplied voltage is out of the range (operating volatage set in the control table)
 46 |     static const uint8_t ERRBIT_ANGLE = 2;         // Goal position is written out of the range (from CW angle limit to CCW angle limit)
 47 |     static const uint8_t ERRBIT_OVERHEAT = 4;      // Temperature is out of the range (operating temperature set in the control table)
 48 |     static const uint8_t ERRBIT_RANGE = 8;         // Command(setting value) is out of the range for use.
 49 |     static const uint8_t ERRBIT_CHECKSUM = 16;     // Instruction packet checksum is incorrect.
 50 |     static const uint8_t ERRBIT_OVERLOAD = 32;     // The current load cannot be controlled by the set torque.
 51 |     static const uint8_t ERRBIT_INSTRUCTION = 64;  // Undefined instruction or delivering the action command without the reg_write command.
 52 | 
 53 | public:
 54 |     virtual ~Protocol1PacketHandler() {}
 55 | 
 56 |     float getProtocolVersion() { return 1.0; }
 57 | 
 58 |     const char *getRxPacketError(uint8_t error) {
 59 | #ifndef __AVR__
 60 |         if (error & ERRBIT_VOLTAGE) return "[RxPacketError] Input voltage error!";
 61 |         if (error & ERRBIT_ANGLE) return "[RxPacketError] Angle limit error!";
 62 |         if (error & ERRBIT_OVERHEAT) return "[RxPacketError] Overheat error!";
 63 |         if (error & ERRBIT_RANGE) return "[RxPacketError] Out of range error!";
 64 |         if (error & ERRBIT_CHECKSUM) return "[RxPacketError] Checksum error!";
 65 |         if (error & ERRBIT_OVERLOAD) return "[RxPacketError] Overload error!";
 66 |         if (error & ERRBIT_INSTRUCTION) return "[RxPacketError] Instruction code error!";
 67 | #else
 68 |         (void)error;
 69 | #endif
 70 |         return "";
 71 |     }
 72 | 
 73 |     int txPacket(uint8_t *txpacket) {
 74 |         uint8_t checksum = 0;
 75 |         uint16_t total_packet_length = txpacket[PKT_LENGTH] + 4;  // 4: HEADER0 HEADER1 ID LENGTH
 76 |         uint8_t written_packet_length = 0;
 77 | 
 78 |         // check max packet length
 79 |         if (total_packet_length > TXPACKET_MAX_LEN) return COMM_TX_ERROR;
 80 | 
 81 |         // make packet header
 82 |         txpacket[PKT_HEADER0] = 0xFF;
 83 |         txpacket[PKT_HEADER1] = 0xFF;
 84 | 
 85 |         // add a checksum to the packet
 86 |         for (uint16_t idx = 2; idx < total_packet_length - 1; idx++)  // except header, checksum
 87 |             checksum += txpacket[idx];
 88 |         txpacket[total_packet_length - 1] = ~checksum;
 89 | 
 90 |         // tx packet
 91 |         port->clearPort();
 92 |         written_packet_length = port->writePort(txpacket, total_packet_length);
 93 |         if (total_packet_length != written_packet_length)
 94 |             return COMM_TX_FAIL;
 95 | 
 96 |         return COMM_SUCCESS;
 97 |     }
 98 | 
 99 |     int rxPacket(uint8_t *rxpacket) {
100 |         int result = COMM_TX_FAIL;
101 |         uint8_t checksum = 0;
102 |         uint16_t rx_length = 0;
103 |         uint16_t wait_length = 6;  // minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM)
104 | 
105 |         while (true) {
106 |             rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length);
107 |             if (rx_length >= wait_length) {
108 |                 uint8_t idx = 0;
109 | 
110 |                 // find packet header
111 |                 for (idx = 0; idx < (rx_length - 1); idx++) {
112 |                     if (rxpacket[idx] == 0xFF && rxpacket[idx + 1] == 0xFF)
113 |                         break;
114 |                 }
115 | 
116 |                 if (idx == 0)  // found at the beginning of the packet
117 |                 {
118 |                     if (rxpacket[PKT_ID] > 0xFD ||                  // unavailable ID
119 |                         rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN ||  // unavailable Length
120 |                         rxpacket[PKT_ERROR] > 0x7F)                 // unavailable Error
121 |                     {
122 |                         // remove the first byte in the packet
123 |                         for (uint16_t s = 0; s < rx_length - 1; s++)
124 |                             rxpacket[s] = rxpacket[1 + s];
125 |                         rx_length -= 1;
126 |                         continue;
127 |                     }
128 | 
129 |                     // re-calculate the exact length of the rx packet
130 |                     if (wait_length != (uint16_t)(rxpacket[PKT_LENGTH] + PKT_LENGTH + 1)) {
131 |                         wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1;
132 |                         continue;
133 |                     }
134 | 
135 |                     if (rx_length < wait_length) {
136 |                         // check timeout
137 |                         if (port->isPacketTimeout() == true) {
138 |                             if (rx_length == 0)
139 |                                 result = COMM_RX_TIMEOUT;
140 |                             else
141 |                                 result = COMM_RX_CORRUPT;
142 |                             break;
143 |                         } else
144 |                             continue;
145 |                     }
146 | 
147 |                     // calculate checksum
148 |                     for (uint16_t i = 2; i < wait_length - 1; i++)  // except header, checksum
149 |                         checksum += rxpacket[i];
150 |                     checksum = ~checksum;
151 | 
152 |                     // verify checksum
153 |                     if (rxpacket[wait_length - 1] == checksum)
154 |                         result = COMM_SUCCESS;
155 |                     else
156 |                         result = COMM_RX_CORRUPT;
157 |                     break;
158 |                 } else {
159 |                     // remove unnecessary packets
160 |                     for (uint16_t s = 0; s < rx_length - idx; s++)
161 |                         rxpacket[s] = rxpacket[idx + s];
162 |                     rx_length -= idx;
163 |                 }
164 |             } else {
165 |                 if (port->isPacketTimeout() == true) {
166 |                     if (rx_length == 0)
167 |                         result = COMM_RX_TIMEOUT;
168 |                     else
169 |                         result = COMM_RX_CORRUPT;
170 |                     break;
171 |                 }
172 |             }
173 |         }
174 | 
175 |         return result;
176 |     }
177 | 
178 |     int txRxPacket(uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0) {
179 |         int result = COMM_TX_FAIL;
180 | 
181 |         // tx packet
182 |         result = txPacket(txpacket);
183 |         if (result != COMM_SUCCESS)
184 |             return result;
185 | 
186 |         // (Instruction == BulkRead) == this function is not available.
187 |         if (txpacket[PKT_INSTRUCTION] == INST_BULK_READ)
188 |             result = COMM_NOT_AVAILABLE;
189 | 
190 |         // (ID == Broadcast ID) == no need to wait for status packet or not available
191 |         // (Instruction == action) == no need to wait for status packet
192 |         if (txpacket[PKT_ID] == BROADCAST_ID || txpacket[PKT_INSTRUCTION] == INST_ACTION)
193 |             return result;
194 | 
195 |         // set packet timeout
196 |         if (txpacket[PKT_INSTRUCTION] == INST_READ)
197 |             port->setPacketTimeout((uint16_t)(txpacket[PKT_PARAMETER0 + 1] + 6));
198 |         else
199 |             port->setPacketTimeout((uint16_t)6);  // HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM
200 | 
201 |         // rx packet
202 |         do {
203 |             result = rxPacket(rxpacket);
204 |         } while (result == COMM_SUCCESS && txpacket[PKT_ID] != rxpacket[PKT_ID]);
205 | 
206 |         if (result == COMM_SUCCESS && txpacket[PKT_ID] == rxpacket[PKT_ID]) {
207 |             if (error != 0)
208 |                 *error = (uint8_t)rxpacket[PKT_ERROR];
209 |         }
210 | 
211 |         return result;
212 |     }
213 | 
214 |     int ping(uint8_t id, uint16_t *model_number, uint8_t *error = 0) {
215 |         (void)model_number;
216 | 
217 |         int result = COMM_TX_FAIL;
218 | 
219 |         uint8_t txpacket[6] = {0};
220 |         uint8_t rxpacket[6] = {0};
221 | 
222 |         if (id >= BROADCAST_ID)
223 |             return COMM_NOT_AVAILABLE;
224 | 
225 |         txpacket[PKT_ID] = id;
226 |         txpacket[PKT_LENGTH] = 2;
227 |         txpacket[PKT_INSTRUCTION] = INST_PING;
228 | 
229 |         result = txRxPacket(txpacket, rxpacket, error);
230 |         // no model number in Protocol 1.0
231 | 
232 |         return result;
233 |     }
234 | 
235 |     int broadcastPing(Vec<uint8_t> &id_list) {
236 |         (void)id_list;
237 |         return COMM_NOT_AVAILABLE;
238 |     }
239 | 
240 |     int action(uint8_t id) {
241 |         uint8_t txpacket[6] = {0};
242 |         txpacket[PKT_ID] = id;
243 |         txpacket[PKT_LENGTH] = 2;
244 |         txpacket[PKT_INSTRUCTION] = INST_ACTION;
245 |         return txRxPacket(txpacket, 0);
246 |     }
247 | 
248 |     int reboot(uint8_t id, uint8_t *error = 0) {
249 |         (void)id;
250 |         (void)error;
251 |         return COMM_NOT_AVAILABLE;
252 |     }
253 | 
254 |     int clearMultiTurn(uint8_t id, uint8_t *error = 0) {
255 |         (void)id;
256 |         (void)error;
257 |         return COMM_NOT_AVAILABLE;
258 |     }
259 | 
260 |     int factoryReset(uint8_t id, uint8_t option, uint8_t *error = 0) {
261 |         (void)option;
262 |         uint8_t txpacket[6] = {0};
263 |         uint8_t rxpacket[6] = {0};
264 |         txpacket[PKT_ID] = id;
265 |         txpacket[PKT_LENGTH] = 2;
266 |         txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET;
267 |         return txRxPacket(txpacket, rxpacket, error);
268 |     }
269 | 
270 |     int readRx(uint8_t id, uint16_t length, uint8_t *data, uint8_t *error = 0) {
271 |         int result = COMM_TX_FAIL;
272 |         uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);  //(length+6);
273 | 
274 |         do {
275 |             result = rxPacket(rxpacket);
276 |         } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id);
277 | 
278 |         if (result == COMM_SUCCESS && rxpacket[PKT_ID] == id) {
279 |             if (error != 0)
280 |                 *error = (uint8_t)rxpacket[PKT_ERROR];
281 | 
282 |             for (uint16_t s = 0; s < length; s++) {
283 |                 data[s] = rxpacket[PKT_PARAMETER0 + s];
284 |             }
285 |         }
286 | 
287 |         free(rxpacket);
288 |         return result;
289 |     }
290 | 
291 |     int readTxRx(uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) {
292 |         int result = COMM_TX_FAIL;
293 |         uint8_t txpacket[8] = {0};
294 |         uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);  //(length+6);
295 | 
296 |         if (id >= BROADCAST_ID) return COMM_NOT_AVAILABLE;
297 | 
298 |         txpacket[PKT_ID] = id;
299 |         txpacket[PKT_LENGTH] = 4;
300 |         txpacket[PKT_INSTRUCTION] = INST_READ;
301 |         txpacket[PKT_PARAMETER0 + 0] = (uint8_t)address;
302 |         txpacket[PKT_PARAMETER0 + 1] = (uint8_t)length;
303 | 
304 |         result = txRxPacket(txpacket, rxpacket, error);
305 |         if (result == COMM_SUCCESS) {
306 |             if (error != 0)
307 |                 *error = (uint8_t)rxpacket[PKT_ERROR];
308 |             for (uint16_t s = 0; s < length; s++)
309 |                 data[s] = rxpacket[PKT_PARAMETER0 + s];
310 |         }
311 | 
312 |         free(rxpacket);
313 |         return result;
314 |     }
315 | 
316 |     int writeTxRx(uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) {
317 |         int result = COMM_TX_FAIL;
318 | 
319 |         uint8_t *txpacket = (uint8_t *)malloc(length + 7);  //#6->7
320 |         uint8_t rxpacket[6] = {0};
321 | 
322 |         txpacket[PKT_ID] = id;
323 |         txpacket[PKT_LENGTH] = length + 3;
324 |         txpacket[PKT_INSTRUCTION] = INST_WRITE;
325 |         txpacket[PKT_PARAMETER0] = (uint8_t)address;
326 | 
327 |         for (uint16_t s = 0; s < length; s++)
328 |             txpacket[PKT_PARAMETER0 + 1 + s] = data[s];
329 | 
330 |         result = txRxPacket(txpacket, rxpacket, error);
331 | 
332 |         free(txpacket);
333 |         return result;
334 |     }
335 | 
336 |     int regWriteTxRx(uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) {
337 |         int result = COMM_TX_FAIL;
338 | 
339 |         uint8_t *txpacket = (uint8_t *)malloc(length + 6);
340 |         uint8_t rxpacket[6] = {0};
341 | 
342 |         txpacket[PKT_ID] = id;
343 |         txpacket[PKT_LENGTH] = length + 3;
344 |         txpacket[PKT_INSTRUCTION] = INST_REG_WRITE;
345 |         txpacket[PKT_PARAMETER0] = (uint8_t)address;
346 | 
347 |         for (uint16_t s = 0; s < length; s++)
348 |             txpacket[PKT_PARAMETER0 + 1 + s] = data[s];
349 | 
350 |         result = txRxPacket(txpacket, rxpacket, error);
351 | 
352 |         free(txpacket);
353 |         return result;
354 |     }
355 | };
356 | 
357 | }  // namespace dynamixel
358 | 
359 | #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ */
360 | 


--------------------------------------------------------------------------------
/Dynamixel/lib/DynamixelSDK/include/protocol2_packet_handler.h:
--------------------------------------------------------------------------------
  1 | // 2019.04.01 by Hideaki Tai : simplified to adapt only to Arduino
  2 | /*******************************************************************************
  3 | * Copyright 2017 ROBOTIS CO., LTD.
  4 | *
  5 | * Licensed under the Apache License, Version 2.0 (the "License");
  6 | * you may not use this file except in compliance with the License.
  7 | * You may obtain a copy of the License at
  8 | *
  9 | *     http://www.apache.org/licenses/LICENSE-2.0
 10 | *
 11 | * Unless required by applicable law or agreed to in writing, software
 12 | * distributed under the License is distributed on an "AS IS" BASIS,
 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 14 | * See the License for the specific language governing permissions and
 15 | * limitations under the License.
 16 | *******************************************************************************/
 17 | 
 18 | ////////////////////////////////////////////////////////////////////////////////
 19 | /// @file The file for Protocol 2.0 Dynamixel packet control
 20 | /// @author Zerom, Leon (RyuWoon Jung)
 21 | ////////////////////////////////////////////////////////////////////////////////
 22 | 
 23 | #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_
 24 | #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_
 25 | 
 26 | #include "types.h"
 27 | #include "packet_handler.h"
 28 | 
 29 | namespace dynamixel {
 30 | 
 31 | #ifdef __AVR__
 32 | static constexpr uint16_t crc_table[256] PROGMEM = {
 33 | #else
 34 | static constexpr uint16_t crc_table[256] = {
 35 | #endif
 36 |     0x0000,
 37 |     0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
 38 |     0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027,
 39 |     0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D,
 40 |     0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B,
 41 |     0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9,
 42 |     0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF,
 43 |     0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5,
 44 |     0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093,
 45 |     0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
 46 |     0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197,
 47 |     0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE,
 48 |     0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB,
 49 |     0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9,
 50 |     0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F,
 51 |     0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176,
 52 |     0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123,
 53 |     0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
 54 |     0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104,
 55 |     0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D,
 56 |     0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B,
 57 |     0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A,
 58 |     0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C,
 59 |     0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5,
 60 |     0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3,
 61 |     0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
 62 |     0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7,
 63 |     0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E,
 64 |     0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B,
 65 |     0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9,
 66 |     0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC,
 67 |     0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5,
 68 |     0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243,
 69 |     0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
 70 |     0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264,
 71 |     0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E,
 72 |     0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208,
 73 |     0x820D, 0x8207, 0x0202};
 74 | 
 75 | inline uint16_t updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size) {
 76 |     uint16_t i = 0;
 77 |     for (uint16_t j = 0; j < data_blk_size; j++) {
 78 |         i = ((uint16_t)(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF;
 79 | #ifdef __AVR__
 80 |         crc_accum = (crc_accum << 8) ^ pgm_read_byte_near(crc_table + i);
 81 | #else
 82 |         crc_accum = (crc_accum << 8) ^ crc_table[i];
 83 | #endif
 84 |     }
 85 | 
 86 |     return crc_accum;
 87 | }
 88 | 
 89 | class Protocol2PacketHandler : public PacketHandler {
 90 |     static const size_t TXPACKET_MAX_LEN = 1 * 1024;
 91 |     static const size_t RXPACKET_MAX_LEN = 1 * 1024;
 92 | 
 93 |     ///////////////// for Protocol 2.0 Packet /////////////////
 94 |     static const uint8_t PKT_HEADER0 = 0;
 95 |     static const uint8_t PKT_HEADER1 = 1;
 96 |     static const uint8_t PKT_HEADER2 = 2;
 97 |     static const uint8_t PKT_RESERVED = 3;
 98 |     static const uint8_t PKT_ID = 4;
 99 |     static const uint8_t PKT_LENGTH_L = 5;
100 |     static const uint8_t PKT_LENGTH_H = 6;
101 |     static const uint8_t PKT_INSTRUCTION = 7;
102 |     static const uint8_t PKT_ERROR = 8;
103 |     static const uint8_t PKT_PARAMETER0 = 8;
104 | 
105 |     ///////////////// Protocol 2.0 Error bit /////////////////
106 |     static const uint8_t ERRNUM_RESULT_FAIL = 1;  // Failed to process the instruction packet.
107 |     static const uint8_t ERRNUM_INSTRUCTION = 2;  // Instruction error
108 |     static const uint8_t ERRNUM_CRC = 3;          // CRC check error
109 |     static const uint8_t ERRNUM_DATA_RANGE = 4;   // Data range error
110 |     static const uint8_t ERRNUM_DATA_LENGTH = 5;  // Data length error
111 |     static const uint8_t ERRNUM_DATA_LIMIT = 6;   // Data limit error
112 |     static const uint8_t ERRNUM_ACCESS = 7;       // Access error
113 |     static const uint8_t ERRBIT_ALERT = 128;      //When the device has a problem, this bit is set to 1. Check "Device Status Check" value.
114 | 
115 |     void addStuffing(uint8_t *packet) {
116 |         int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]);
117 |         int packet_length_out = packet_length_in;
118 | 
119 |         if (packet_length_in < 8)  // INSTRUCTION, ADDR_L, ADDR_H, CRC16_L, CRC16_H + FF FF FD
120 |             return;
121 | 
122 |         uint8_t *packet_ptr;
123 |         uint16_t packet_length_before_crc = packet_length_in - 2;
124 |         for (uint16_t i = 3; i < packet_length_before_crc; i++) {
125 |             packet_ptr = &packet[i + PKT_INSTRUCTION - 2];
126 |             if (packet_ptr[0] == 0xFF && packet_ptr[1] == 0xFF && packet_ptr[2] == 0xFD)
127 |                 packet_length_out++;
128 |         }
129 | 
130 |         if (packet_length_in == packet_length_out)  // no stuffing required
131 |             return;
132 | 
133 |         uint16_t out_index = packet_length_out + 6 - 2;  // last index before crc
134 |         uint16_t in_index = packet_length_in + 6 - 2;    // last index before crc
135 |         while (out_index != in_index) {
136 |             if (packet[in_index] == 0xFD && packet[in_index - 1] == 0xFF && packet[in_index - 2] == 0xFF) {
137 |                 packet[out_index--] = 0xFD;  // byte stuffing
138 |                 if (out_index != in_index) {
139 |                     packet[out_index--] = packet[in_index--];  // FD
140 |                     packet[out_index--] = packet[in_index--];  // FF
141 |                     packet[out_index--] = packet[in_index--];  // FF
142 |                 }
143 |             } else {
144 |                 packet[out_index--] = packet[in_index--];
145 |             }
146 |         }
147 | 
148 |         packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out);
149 |         packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out);
150 | 
151 |         return;
152 |     }
153 | 
154 |     void removeStuffing(uint8_t *packet) {
155 |         int i = 0, index = 0;
156 |         int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]);
157 |         int packet_length_out = packet_length_in;
158 | 
159 |         index = PKT_INSTRUCTION;
160 |         for (i = 0; i < packet_length_in - 2; i++)  // except CRC
161 |         {
162 |             if (packet[i + PKT_INSTRUCTION] == 0xFD && packet[i + PKT_INSTRUCTION + 1] == 0xFD && packet[i + PKT_INSTRUCTION - 1] == 0xFF && packet[i + PKT_INSTRUCTION - 2] == 0xFF) {  // FF FF FD FD
163 |                 packet_length_out--;
164 |                 i++;
165 |             }
166 |             packet[index++] = packet[i + PKT_INSTRUCTION];
167 |         }
168 |         packet[index++] = packet[PKT_INSTRUCTION + packet_length_in - 2];
169 |         packet[index++] = packet[PKT_INSTRUCTION + packet_length_in - 1];
170 | 
171 |         packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out);
172 |         packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out);
173 |     }
174 | 
175 | public:
176 |     ~Protocol2PacketHandler() {}
177 | 
178 |     float getProtocolVersion() { return 2.0; }
179 | 
180 |     const char *getRxPacketError(uint8_t error) {
181 | #ifndef __AVR__
182 |         if (error & ERRBIT_ALERT)
183 |             return "[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!";
184 | 
185 |         const uint8_t not_alert_error = error & ~ERRBIT_ALERT;
186 | 
187 |         switch (not_alert_error) {
188 |             case 0:
189 |                 return "";
190 |             case ERRNUM_RESULT_FAIL:
191 |                 return "[RxPacketError] Failed to process the instruction packet!";
192 |             case ERRNUM_INSTRUCTION:
193 |                 return "[RxPacketError] Undefined instruction or incorrect instruction!";
194 |             case ERRNUM_CRC:
195 |                 return "[RxPacketError] CRC doesn't match!";
196 |             case ERRNUM_DATA_RANGE:
197 |                 return "[RxPacketError] The data value is out of range!";
198 |             case ERRNUM_DATA_LENGTH:
199 |                 return "[RxPacketError] The data length does not match as expected!";
200 |             case ERRNUM_DATA_LIMIT:
201 |                 return "[RxPacketError] The data value exceeds the limit value!";
202 |             case ERRNUM_ACCESS:
203 |                 return "[RxPacketError] Writing or Reading is not available to target address!";
204 |             default:
205 |                 return "[RxPacketError] Unknown error code!";
206 |         }
207 | #else
208 |         (void)error;
209 |         return "";
210 | #endif
211 |     }
212 | 
213 |     int txPacket(uint8_t *txpacket) {
214 |         uint16_t total_packet_length = 0;
215 |         uint16_t written_packet_length = 0;
216 | 
217 |         addStuffing(txpacket);
218 | 
219 |         total_packet_length = DXL_MAKEWORD(txpacket[PKT_LENGTH_L], txpacket[PKT_LENGTH_H]) + 7;
220 |         // 7: HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H
221 |         if (total_packet_length > TXPACKET_MAX_LEN)
222 |             return COMM_TX_ERROR;
223 | 
224 |         txpacket[PKT_HEADER0] = 0xFF;
225 |         txpacket[PKT_HEADER1] = 0xFF;
226 |         txpacket[PKT_HEADER2] = 0xFD;
227 |         txpacket[PKT_RESERVED] = 0x00;
228 | 
229 |         uint16_t crc = updateCRC(0, txpacket, total_packet_length - 2);  // 2: CRC16
230 |         txpacket[total_packet_length - 2] = DXL_LOBYTE(crc);
231 |         txpacket[total_packet_length - 1] = DXL_HIBYTE(crc);
232 | 
233 |         // tx packet
234 |         port->clearPort();
235 |         written_packet_length = port->writePort(txpacket, total_packet_length);
236 |         if (total_packet_length != written_packet_length)
237 |             return COMM_TX_FAIL;
238 | 
239 |         return COMM_SUCCESS;
240 |     }
241 | 
242 |     int rxPacket(uint8_t *rxpacket) {
243 |         int result = COMM_TX_FAIL;
244 | 
245 |         uint16_t rx_length = 0;
246 |         uint16_t wait_length = 11;  // minimum length (HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H)
247 | 
248 |         while (true) {
249 |             rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length);
250 |             if (rx_length >= wait_length) {
251 |                 uint16_t idx = 0;
252 | 
253 |                 // find packet header
254 |                 for (idx = 0; idx < (rx_length - 3); idx++) {
255 |                     if ((rxpacket[idx] == 0xFF) && (rxpacket[idx + 1] == 0xFF) && (rxpacket[idx + 2] == 0xFD) && (rxpacket[idx + 3] != 0xFD))
256 |                         break;
257 |                 }
258 | 
259 |                 if (idx == 0)  // found at the beginning of the packet
260 |                 {
261 |                     if (rxpacket[PKT_RESERVED] != 0x00 ||
262 |                         rxpacket[PKT_ID] > 0xFC ||
263 |                         DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) > RXPACKET_MAX_LEN ||
264 |                         rxpacket[PKT_INSTRUCTION] != 0x55) {
265 |                         // remove the first byte in the packet
266 |                         for (uint16_t s = 0; s < rx_length - 1; s++)
267 |                             rxpacket[s] = rxpacket[1 + s];
268 |                         rx_length -= 1;
269 |                         continue;
270 |                     }
271 | 
272 |                     // re-calculate the exact length of the rx packet
273 |                     if (wait_length != DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1) {
274 |                         wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1;
275 |                         continue;
276 |                     }
277 | 
278 |                     if (rx_length < wait_length) {
279 |                         if (port->isPacketTimeout() == true) {
280 |                             if (rx_length == 0)
281 |                                 result = COMM_RX_TIMEOUT;
282 |                             else
283 |                                 result = COMM_RX_CORRUPT;
284 |                             break;
285 |                         } else
286 |                             continue;
287 |                     }
288 | 
289 |                     uint16_t crc = DXL_MAKEWORD(rxpacket[wait_length - 2], rxpacket[wait_length - 1]);
290 |                     if (updateCRC(0, rxpacket, wait_length - 2) == crc)
291 |                         result = COMM_SUCCESS;
292 |                     else
293 |                         result = COMM_RX_CORRUPT;
294 |                     break;
295 |                 } else {
296 |                     // remove unnecessary packets
297 |                     for (uint16_t s = 0; s < rx_length - idx; s++)
298 |                         rxpacket[s] = rxpacket[idx + s];
299 |                     rx_length -= idx;
300 |                 }
301 |             } else {
302 |                 if (port->isPacketTimeout() == true) {
303 |                     if (rx_length == 0)
304 |                         result = COMM_RX_TIMEOUT;
305 |                     else
306 |                         result = COMM_RX_CORRUPT;
307 |                     break;
308 |                 }
309 |             }
310 |         }
311 | 
312 |         if (result == COMM_SUCCESS) removeStuffing(rxpacket);
313 | 
314 |         return result;
315 |     }
316 | 
317 |     int txRxPacket(uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0) {
318 |         int result = COMM_TX_FAIL;
319 | 
320 |         result = txPacket(txpacket);
321 |         if (result != COMM_SUCCESS) return result;
322 | 
323 |         // (Instruction == BulkRead or SyncRead) == this function is not available.
324 |         if (txpacket[PKT_INSTRUCTION] == INST_BULK_READ || txpacket[PKT_INSTRUCTION] == INST_SYNC_READ)
325 |             result = COMM_NOT_AVAILABLE;
326 | 
327 |         // (ID == Broadcast ID) == no need to wait for status packet or not available.
328 |         // (Instruction == action) == no need to wait for status packet
329 |         if (txpacket[PKT_ID] == BROADCAST_ID || txpacket[PKT_INSTRUCTION] == INST_ACTION)
330 |             return result;
331 | 
332 |         // set packet timeout
333 |         if (txpacket[PKT_INSTRUCTION] == INST_READ)
334 |             port->setPacketTimeout((uint16_t)(DXL_MAKEWORD(txpacket[PKT_PARAMETER0 + 2], txpacket[PKT_PARAMETER0 + 3]) + 11));
335 |         else
336 |             port->setPacketTimeout((uint16_t)11);
337 |         // HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H
338 | 
339 |         // rx packet
340 |         do {
341 |             result = rxPacket(rxpacket);
342 |         } while (result == COMM_SUCCESS && txpacket[PKT_ID] != rxpacket[PKT_ID]);
343 | 
344 |         if (result == COMM_SUCCESS && txpacket[PKT_ID] == rxpacket[PKT_ID])
345 |             if (error != 0) *error = (uint8_t)rxpacket[PKT_ERROR];
346 | 
347 |         return result;
348 |     }
349 | 
350 |     int ping(uint8_t id, uint16_t *model_number, uint8_t *error = 0) {
351 |         if (id >= BROADCAST_ID) return COMM_NOT_AVAILABLE;
352 | 
353 |         uint8_t txpacket[10] = {0};
354 |         uint8_t rxpacket[14] = {0};
355 | 
356 |         txpacket[PKT_ID] = id;
357 |         txpacket[PKT_LENGTH_L] = 3;
358 |         txpacket[PKT_LENGTH_H] = 0;
359 |         txpacket[PKT_INSTRUCTION] = INST_PING;
360 | 
361 |         int result = txRxPacket(txpacket, rxpacket, error);
362 |         if (result == COMM_SUCCESS && model_number != 0)
363 |             *model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0 + 1], rxpacket[PKT_PARAMETER0 + 2]);
364 | 
365 |         return result;
366 |     }
367 | 
368 |     int broadcastPing(Vec<uint8_t> &id_list) {
369 |         const int STATUS_LENGTH = 14;
370 |         int result = COMM_TX_FAIL;
371 | 
372 |         id_list.clear();
373 | 
374 |         uint16_t rx_length = 0;
375 |         uint16_t wait_length = STATUS_LENGTH * MAX_ID;
376 | 
377 |         uint8_t txpacket[10] = {0};
378 |         uint8_t rxpacket[STATUS_LENGTH * MAX_ID] = {0};
379 | 
380 |         txpacket[PKT_ID] = BROADCAST_ID;
381 |         txpacket[PKT_LENGTH_L] = 3;
382 |         txpacket[PKT_LENGTH_H] = 0;
383 |         txpacket[PKT_INSTRUCTION] = INST_PING;
384 | 
385 |         result = txPacket(txpacket);
386 |         if (result != COMM_SUCCESS) return result;
387 | 
388 |         // set rx timeout
389 |         port->setPacketTimeout((uint16_t)(wait_length * 30));
390 | 
391 |         while (1) {
392 |             rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length);
393 |             if (port->isPacketTimeout() == true)  // || rx_length >= wait_length)
394 |                 break;
395 |         }
396 | 
397 |         if (rx_length == 0) return COMM_RX_TIMEOUT;
398 | 
399 |         while (1) {
400 |             if (rx_length < STATUS_LENGTH)
401 |                 return COMM_RX_CORRUPT;
402 | 
403 |             uint16_t idx = 0;
404 | 
405 |             // find packet header
406 |             for (idx = 0; idx < (rx_length - 2); idx++) {
407 |                 if (rxpacket[idx] == 0xFF && rxpacket[idx + 1] == 0xFF && rxpacket[idx + 2] == 0xFD)
408 |                     break;
409 |             }
410 | 
411 |             if (idx == 0)  // found at the beginning of the packet
412 |             {
413 |                 uint16_t crc = DXL_MAKEWORD(rxpacket[STATUS_LENGTH - 2], rxpacket[STATUS_LENGTH - 1]);
414 | 
415 |                 if (updateCRC(0, rxpacket, STATUS_LENGTH - 2) == crc) {
416 |                     result = COMM_SUCCESS;
417 |                     id_list.push_back(rxpacket[PKT_ID]);
418 |                     for (uint16_t s = 0; s < rx_length - STATUS_LENGTH; s++)
419 |                         rxpacket[s] = rxpacket[STATUS_LENGTH + s];
420 |                     rx_length -= STATUS_LENGTH;
421 |                     if (rx_length == 0) return result;
422 |                 } else {
423 |                     result = COMM_RX_CORRUPT;
424 |                     // remove header (0xFF 0xFF 0xFD)
425 |                     for (uint16_t s = 0; s < rx_length - 3; s++)
426 |                         rxpacket[s] = rxpacket[3 + s];
427 |                     rx_length -= 3;
428 |                 }
429 |             } else {
430 |                 // remove unnecessary packets
431 |                 for (uint16_t s = 0; s < rx_length - idx; s++)
432 |                     rxpacket[s] = rxpacket[idx + s];
433 |                 rx_length -= idx;
434 |             }
435 |         }
436 |         return result;
437 |     }
438 | 
439 |     int action(uint8_t id) {
440 |         uint8_t txpacket[10] = {0};
441 | 
442 |         txpacket[PKT_ID] = id;
443 |         txpacket[PKT_LENGTH_L] = 3;
444 |         txpacket[PKT_LENGTH_H] = 0;
445 |         txpacket[PKT_INSTRUCTION] = INST_ACTION;
446 | 
447 |         return txRxPacket(txpacket, 0);
448 |     }
449 | 
450 |     int reboot(uint8_t id, uint8_t *error = 0) {
451 |         uint8_t txpacket[10] = {0};
452 |         uint8_t rxpacket[11] = {0};
453 | 
454 |         txpacket[PKT_ID] = id;
455 |         txpacket[PKT_LENGTH_L] = 3;
456 |         txpacket[PKT_LENGTH_H] = 0;
457 |         txpacket[PKT_INSTRUCTION] = INST_REBOOT;
458 | 
459 |         return txRxPacket(txpacket, rxpacket, error);
460 |     }
461 | 
462 |     int clearMultiTurn(uint8_t id, uint8_t *error = 0) {
463 |         uint8_t txpacket[15] = {0};
464 |         uint8_t rxpacket[11] = {0};
465 | 
466 |         txpacket[PKT_ID] = id;
467 |         txpacket[PKT_LENGTH_L] = 8;
468 |         txpacket[PKT_LENGTH_H] = 0;
469 |         txpacket[PKT_INSTRUCTION] = INST_CLEAR;
470 |         txpacket[PKT_PARAMETER0] = 0x01;
471 |         txpacket[PKT_PARAMETER0 + 1] = 0x44;
472 |         txpacket[PKT_PARAMETER0 + 2] = 0x58;
473 |         txpacket[PKT_PARAMETER0 + 3] = 0x4C;
474 |         txpacket[PKT_PARAMETER0 + 4] = 0x22;
475 | 
476 |         return txRxPacket(txpacket, rxpacket, error);
477 |     }
478 | 
479 |     int factoryReset(uint8_t id, uint8_t option, uint8_t *error = 0) {
480 |         uint8_t txpacket[11] = {0};
481 |         uint8_t rxpacket[11] = {0};
482 | 
483 |         txpacket[PKT_ID] = id;
484 |         txpacket[PKT_LENGTH_L] = 4;
485 |         txpacket[PKT_LENGTH_H] = 0;
486 |         txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET;
487 |         txpacket[PKT_PARAMETER0] = option;
488 | 
489 |         return txRxPacket(txpacket, rxpacket, error);
490 |     }
491 | 
492 |     int readRx(uint8_t id, uint16_t length, uint8_t *data, uint8_t *error = 0) {
493 |         int result = COMM_TX_FAIL;
494 |         uint8_t *rxpacket = (uint8_t *)malloc(length + 11 + (length / 3));
495 | 
496 |         do {
497 |             result = rxPacket(rxpacket);
498 |         } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id);
499 | 
500 |         if (result == COMM_SUCCESS && rxpacket[PKT_ID] == id) {
501 |             if (error != 0)
502 |                 *error = (uint8_t)rxpacket[PKT_ERROR];
503 |             for (uint16_t s = 0; s < length; s++)
504 |                 data[s] = rxpacket[PKT_PARAMETER0 + 1 + s];
505 |         }
506 |         free(rxpacket);
507 |         return result;
508 |     }
509 | 
510 |     int readTxRx(uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) {
511 |         int result = COMM_TX_FAIL;
512 |         uint8_t txpacket[14] = {0};
513 |         uint8_t *rxpacket = (uint8_t *)malloc(length + 11 + (length / 3));
514 | 
515 |         if (rxpacket == NULL) return result;
516 |         if (id >= BROADCAST_ID) return COMM_NOT_AVAILABLE;
517 | 
518 |         txpacket[PKT_ID] = id;
519 |         txpacket[PKT_LENGTH_L] = 7;
520 |         txpacket[PKT_LENGTH_H] = 0;
521 |         txpacket[PKT_INSTRUCTION] = INST_READ;
522 |         txpacket[PKT_PARAMETER0 + 0] = (uint8_t)DXL_LOBYTE(address);
523 |         txpacket[PKT_PARAMETER0 + 1] = (uint8_t)DXL_HIBYTE(address);
524 |         txpacket[PKT_PARAMETER0 + 2] = (uint8_t)DXL_LOBYTE(length);
525 |         txpacket[PKT_PARAMETER0 + 3] = (uint8_t)DXL_HIBYTE(length);
526 | 
527 |         result = txRxPacket(txpacket, rxpacket, error);
528 |         if (result == COMM_SUCCESS) {
529 |             if (error != 0)
530 |                 *error = (uint8_t)rxpacket[PKT_ERROR];
531 |             for (uint16_t s = 0; s < length; s++)
532 |                 data[s] = rxpacket[PKT_PARAMETER0 + 1 + s];
533 |         }
534 | 
535 |         free(rxpacket);
536 |         return result;
537 |     }
538 | 
539 |     int writeTxRx(uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) {
540 |         int result = COMM_TX_FAIL;
541 |         uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3));
542 |         uint8_t rxpacket[11] = {0};
543 | 
544 |         if (txpacket == NULL) return result;
545 | 
546 |         txpacket[PKT_ID] = id;
547 |         txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length + 5);
548 |         txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length + 5);
549 |         txpacket[PKT_INSTRUCTION] = INST_WRITE;
550 |         txpacket[PKT_PARAMETER0 + 0] = (uint8_t)DXL_LOBYTE(address);
551 |         txpacket[PKT_PARAMETER0 + 1] = (uint8_t)DXL_HIBYTE(address);
552 | 
553 |         for (uint16_t s = 0; s < length; s++)
554 |             txpacket[PKT_PARAMETER0 + 2 + s] = data[s];
555 | 
556 |         result = txRxPacket(txpacket, rxpacket, error);
557 | 
558 |         free(txpacket);
559 |         return result;
560 |     }
561 | 
562 |     int regWriteTxRx(uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) {
563 |         int result = COMM_TX_FAIL;
564 |         uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3));
565 |         uint8_t rxpacket[11] = {0};
566 | 
567 |         if (txpacket == NULL) return result;
568 | 
569 |         txpacket[PKT_ID] = id;
570 |         txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length + 5);
571 |         txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length + 5);
572 |         txpacket[PKT_INSTRUCTION] = INST_REG_WRITE;
573 |         txpacket[PKT_PARAMETER0 + 0] = (uint8_t)DXL_LOBYTE(address);
574 |         txpacket[PKT_PARAMETER0 + 1] = (uint8_t)DXL_HIBYTE(address);
575 | 
576 |         for (uint16_t s = 0; s < length; s++)
577 |             txpacket[PKT_PARAMETER0 + 2 + s] = data[s];
578 | 
579 |         result = txRxPacket(txpacket, rxpacket, error);
580 | 
581 |         free(txpacket);
582 |         return result;
583 |     }
584 | };
585 | 
586 | }  // namespace dynamixel
587 | 
588 | #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ */
589 | 


--------------------------------------------------------------------------------
/Dynamixel/lib/DynamixelSDK/include/types.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | #ifndef DYNAMIXEL_SDK_TYPES_H
 3 | #define DYNAMIXEL_SDK_TYPES_H
 4 | 
 5 | namespace dynamixel {
 6 | 
 7 | #if ARX_HAVE_LIBSTDCPLUSPLUS >= 201103L  // Have libstdc++11
 8 | template <typename T>
 9 | using Vec = std::vector<T>;
10 | template <typename T, typename U>
11 | using Map = std::map<T, U>;
12 | #else  // Do not have libstdc++11
13 | template <typename T>
14 | using Vec = arx::stdx::vector<T>;
15 | template <typename T, typename U>
16 | using Map = arx::stdx::map<T, U>;
17 | #endif
18 | 
19 | }  // namespace dynamixel
20 | 
21 | #endif  // DYNAMIXEL_SDK_TYPES_H
22 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2019 Hideaki Tai
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | # Dynamixel
  2 | 
  3 | Arduino library for [Dynamixel](http://en.robotis.com/model/page.php?co_id=prd_dynamixel_x).
  4 | 
  5 | ## Feature
  6 | 
  7 | This library is a wrapper of [DynamixelSDK](https://github.com/ROBOTIS-GIT/DynamixelSDK) for Arduino.
  8 | It makes to use Dynamixel easier.
  9 | 
 10 | - Pro, X, MX, and others(DX, RX, AX) series are supported
 11 | - Both Protocol 1.0 and 2.0 are supported (X, MX series supports only 2.0)
 12 | - Almost basic APIs are wrapped and simplified.
 13 | - This library can control multiple motors which is daisy-chained in one RS485 bus.
 14 | - By default, it is assumed that one RTS pin is used.
 15 | - You can also use two pins to control enable/disable RX/TX respectively.
 16 | 
 17 | Please feel free to report issues and PRs.
 18 | 
 19 | ### Supported & Tested Platforms
 20 | 
 21 | This library supports almost all Arduino boards.
 22 | 
 23 | 
 24 | ### Supported Models
 25 | 
 26 | - Pro Series
 27 | - X Series
 28 | - MX Series
 29 | - Others (DX, RX, AX)
 30 | 
 31 | ### API Limitations
 32 | 
 33 | - REG_WRITE/ACTION is not wrapped
 34 | - SYNC READ/WRITE, BULK READ/WRITE is not wrapped
 35 | - INDIRECT ACCESS is not wrapped
 36 | 
 37 | ## Usage
 38 | 
 39 | ``` C++
 40 | #include <Dynamixel.h>
 41 | 
 42 | #define DYNAMIXEL_SERIAL Serial2 // change as you want
 43 | 
 44 | const uint8_t TARGET_ID = 1;
 45 | const uint8_t PIN_RTS = 11;
 46 | const uint16_t DYNAMIXEL_BAUDRATE = 57600;
 47 | 
 48 | Dynamixel dxl(PIN_RTS); // create instance with RTS pin
 49 | 
 50 | int dxl_goal_position[2];
 51 | 
 52 | void setup()
 53 | {
 54 |     // initialize Serial for RS485 and attach it to Dynamixel libaray
 55 |     DYNAMIXEL_SERIAL.begin(DYNAMIXEL_BAUDRATE);
 56 |     dxl.attach(DYNAMIXEL_SERIAL, DYNAMIXEL_BAUDRATE);
 57 | 
 58 |     // add model with id
 59 |     dxl.addModel<DxlModel::X>(TARGET_ID);
 60 |     // dxl.addModel<DxlModel::PRO>(TARGET_ID);
 61 |     // dxl.addModel<DxlModel::MX>(TARGET_ID);
 62 |     // dxl.addModel<DxlModel::OTHER>(TARGET_ID);
 63 | 
 64 |     delay(2000);
 65 | 
 66 |     dxl.torqueEnable(TARGET_ID, false);
 67 | 
 68 |     dxl.minPositionLimit(TARGET_ID, 1400);
 69 |     dxl.maxPositionLimit(TARGET_ID, 1900);
 70 |     dxl_goal_position[0] = dxl.minPositionLimit(TARGET_ID); // get min pos limit
 71 |     dxl_goal_position[1] = dxl.maxPositionLimit(TARGET_ID); // get max pos limit
 72 |     dxl.velocityLimit(TARGET_ID, 30000);
 73 | 
 74 |     dxl.torqueEnable(TARGET_ID, true);
 75 | }
 76 | 
 77 | bool dir = true; // CW or CCW
 78 | 
 79 | void loop()
 80 | {
 81 |     dxl.goalPosition(TARGET_ID, dxl_goal_position[(size_t)dir]); // move to position
 82 | 
 83 |     delay(3000);
 84 | 
 85 |     Serial.print("current pos = ");
 86 |     Serial.println(dxl.presentPosition(TARGET_ID)); // get current position
 87 | 
 88 |     dir = !dir; // reverse direction
 89 | }
 90 | ```
 91 | 
 92 | For more examples, see `examples` folder.
 93 | 
 94 | ## APIs
 95 | 
 96 | ``` C++
 97 | // initialize
 98 | template <Model> void addModel(uint8_t id)
 99 | void attach(Stream& s, size_t baud)
100 | uint8_t size() const
101 | 
102 | // wrapper for instructions
103 | bool ping(uint8_t id)
104 | Models ping() // broadcast
105 | bool factoryReset(uint8_t id, ResetMode mode = ResetMode::EXC_ID_BAUD)
106 | bool factoryReset(ResetMode mode = ResetMode::EXC_ID_BAUD)
107 | bool reboot(uint8_t id)
108 | void verbose(uint8_t id)
109 | bool write(uint8_t id, Reg reg, uint32_t data)
110 | uint32_t read(uint8_t id, Reg reg)
111 | 
112 | // wrappers for control table
113 | // read values
114 | uint16_t modelNumber(uint8_t id)
115 | uint32_t modelInformation(uint8_t id)
116 | uint8_t versionOfFirmware(uint8_t id)
117 | uint8_t id(uint8_t id)
118 | uint8_t baudrate(uint8_t id)
119 | uint8_t returnDelayTime(uint8_t id)
120 | uint8_t driveMode(uint8_t id)
121 | uint8_t operatingMode(uint8_t id)
122 | uint8_t secondaryId(uint8_t id)
123 | uint8_t protocolVersion(uint8_t id)
124 | int32_t homingOffset(uint8_t id)
125 | uint32_t movingThreshold(uint8_t id)
126 | uint8_t temperatureLimit(uint8_t id)
127 | uint16_t maxVoltageLimit(uint8_t id)
128 | uint16_t minVoltageLimit(uint8_t id)
129 | uint16_t pwmLimit(uint8_t id)
130 | uint16_t currentLimit(uint8_t id)
131 | uint32_t accelerationLimit(uint8_t id)
132 | uint32_t velocityLimit(uint8_t id)
133 | uint32_t maxPositionLimit(uint8_t id)
134 | uint32_t minPositionLimit(uint8_t id)
135 | uint8_t shutdown(uint8_t id)
136 | bool torqueEnable(uint8_t id)
137 | uint8_t led(uint8_t id)
138 | uint8_t statusReturnLevel(uint8_t id)
139 | uint8_t registerdInstruction(uint8_t id)
140 | uint8_t hardwareErrorStatus(uint8_t id)
141 | uint16_t velocityIGain(uint8_t id)
142 | uint16_t velocityPGain(uint8_t id)
143 | uint16_t positionDGain(uint8_t id)
144 | uint16_t positionIGain(uint8_t id)
145 | uint16_t positionPGain(uint8_t id)
146 | uint16_t feedForwardAccelerationGain(uint8_t id)
147 | uint16_t feedForwardVelocityGain(uint8_t id)
148 | int8_t busWatchdog(uint8_t id)
149 | int16_t goalPwm(uint8_t id)
150 | int16_t goalCurrent(uint8_t id)
151 | int32_t goalVelocity(uint8_t id)
152 | uint32_t profileAcceleration(uint8_t id)
153 | uint32_t profileVelocity(uint8_t id)
154 | int32_t goalPosition(uint8_t id)
155 | uint16_t realTimeTick(uint8_t id)
156 | uint8_t moving(uint8_t id)
157 | uint8_t movingStatus(uint8_t id)
158 | int16_t presentPwm(uint8_t id)
159 | int16_t presentCurrent(uint8_t id)
160 | int32_t presentVelocity(uint8_t id)
161 | int32_t presentPosition(uint8_t id)
162 | uint32_t velocityTrajectory(uint8_t id)
163 | uint32_t positionTrajectory(uint8_t id)
164 | uint16_t presentInputVoltage(uint8_t id)
165 | uint8_t presentTemperature(uint8_t id)
166 | 
167 | // write values
168 | bool id(uint8_t id, uint8_t x)
169 | bool baudrate(uint8_t id, uint8_t x)
170 | bool returnDelayTime(uint8_t id, uint8_t x)
171 | bool driveMode(uint8_t id, uint8_t x)
172 | bool operatingMode(uint8_t id, uint8_t x)
173 | bool secondaryId(uint8_t id, uint8_t x)
174 | bool protocolVersion(uint8_t id, uint8_t x)
175 | bool homingOffset(uint8_t id, int32_t x)
176 | bool movingThreshold(uint8_t id, uint32_t x)
177 | bool temperatureLimit(uint8_t id, uint8_t x)
178 | bool maxVoltageLimit(uint8_t id, uint16_t x)
179 | bool minVoltageLimit(uint8_t id, uint16_t x)
180 | bool pwmLimit(uint8_t id, uint16_t x)
181 | bool currentLimit(uint8_t id, uint16_t x)
182 | bool accelerationLimit(uint8_t id, uint32_t x)
183 | bool velocityLimit(uint8_t id, uint32_t x)
184 | bool maxPositionLimit(uint8_t id, uint32_t x)
185 | bool minPositionLimit(uint8_t id, uint32_t x)
186 | bool shutdown(uint8_t id, uint8_t x)
187 | bool torqueEnable(uint8_t id, bool x)
188 | bool led(uint8_t id, bool x)
189 | bool statusReturnLevel(uint8_t id, uint8_t x)
190 | bool velocityIGain(uint8_t id, uint16_t x)
191 | bool velocityPGain(uint8_t id, uint16_t x)
192 | bool positionDGain(uint8_t id, uint16_t x)
193 | bool positionIGain(uint8_t id, uint16_t x)
194 | bool positionPGain(uint8_t id, uint16_t x)
195 | bool feedForwardAccelerationGain(uint8_t id, uint16_t x)
196 | bool feedForwardVelocityGain(uint8_t id, uint16_t x)
197 | bool busWatchdog(uint8_t id, int8_t x)
198 | bool goalPwm(uint8_t id, int16_t x)
199 | bool goalCurrent(uint8_t id, int16_t x)
200 | bool goalVelocity(uint8_t id, int32_t x)
201 | bool profileAcceleration(uint8_t id, uint32_t x)
202 | bool profileVelocity(uint8_t id, uint32_t x)
203 | bool goalPosition(uint8_t id, int32_t x)
204 | ```
205 | 
206 | ## TBD
207 | 
208 | - REG_WRITE/ACTION
209 | - INDIRECT ACCESS helper class
210 | - SYNC READ/WRITE
211 | - BULK READ/WRITE
212 | 
213 | 
214 | ## License
215 | 
216 | MIT
217 | 
218 | DynamixelSDK's license is Apache License 2.0.
219 | Please see `Dynamixel/lib/DynamixelSDK` and [DynamixelSDK repo](https://github.com/ROBOTIS-GIT/DynamixelSDK) for details.
220 | 


--------------------------------------------------------------------------------
/examples/WIP_bulk_read_write/bulk_read_write.ino:
--------------------------------------------------------------------------------
  1 | /*******************************************************************************
  2 | * Copyright (c) 2016, ROBOTIS CO., LTD.
  3 | * All rights reserved.
  4 | *
  5 | * Redistribution and use in source and binary forms, with or without
  6 | * modification, are permitted provided that the following conditions are met:
  7 | *
  8 | * * Redistributions of source code must retain the above copyright notice, this
  9 | *   list of conditions and the following disclaimer.
 10 | *
 11 | * * Redistributions in binary form must reproduce the above copyright notice,
 12 | *   this list of conditions and the following disclaimer in the documentation
 13 | *   and/or other materials provided with the distribution.
 14 | *
 15 | * * Neither the name of ROBOTIS nor the names of its
 16 | *   contributors may be used to endorse or promote products derived from
 17 | *   this software without specific prior written permission.
 18 | *
 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 23 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 24 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 25 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 26 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 27 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 28 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 29 | *******************************************************************************/
 30 | 
 31 | /* Author: Ryu Woon Jung (Leon) */
 32 | 
 33 | //
 34 | // *********     Bulk Read and Bulk Write Example      *********
 35 | //
 36 | //
 37 | // Available Dynamixel model on this example : All models using Protocol 2.0
 38 | // This example is tested with two Dynamixel PRO 54-200, and an USB2DYNAMIXEL
 39 | // Be sure that Dynamixel PRO properties are already set as %% ID : 1 and 2 / Baudnum : 1 (Baudrate : 57600)
 40 | //
 41 | 
 42 | #include <DynamixelSDK.h>
 43 | 
 44 | // Control table address
 45 | #define ADDR_PRO_TORQUE_ENABLE          562                 // Control table address is different in Dynamixel model
 46 | #define ADDR_PRO_LED_RED                563
 47 | #define ADDR_PRO_GOAL_POSITION          596
 48 | #define ADDR_PRO_PRESENT_POSITION       611
 49 | 
 50 | // Data Byte Length
 51 | #define LEN_PRO_LED_RED                 1
 52 | #define LEN_PRO_GOAL_POSITION           4
 53 | #define LEN_PRO_PRESENT_POSITION        4
 54 | 
 55 | // Protocol version
 56 | #define PROTOCOL_VERSION                2.0                 // See which protocol version is used in the Dynamixel
 57 | 
 58 | // Default setting
 59 | #define DXL1_ID                         1                   // Dynamixel#1 ID: 1
 60 | #define DXL2_ID                         2                   // Dynamixel#2 ID: 2
 61 | #define BAUDRATE                        57600
 62 | #define DEVICENAME                      "OpenCR_DXL_Port"   // This definition only has a symbolic meaning and does not affect to any functionality
 63 | 
 64 | #define TORQUE_ENABLE                   1                   // Value for enabling the torque
 65 | #define TORQUE_DISABLE                  0                   // Value for disabling the torque
 66 | #define DXL_MINIMUM_POSITION_VALUE     -150000              // Dynamixel will rotate between this value
 67 | #define DXL_MAXIMUM_POSITION_VALUE      150000              // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
 68 | #define DXL_MOVING_STATUS_THRESHOLD     20                  // Dynamixel moving status threshold
 69 | 
 70 | #define ESC_ASCII_VALUE                 0x1b
 71 | 
 72 | #define CMD_SERIAL                      Serial
 73 | 
 74 | 
 75 | #include <ArduinoDynamixel.h>
 76 | 
 77 | const uint8_t PIN_RTS = 11;
 78 | const uint16_t DYNAMIXEL_BAUDRATE = 57600;
 79 | 
 80 | ArduinoDynamixel dx(PIN_RTS);
 81 | 
 82 | void setup()
 83 | {
 84 |     Serial.begin(115200);
 85 | 
 86 |     delay(2000);
 87 | 
 88 |     Serial2.begin(DYNAMIXEL_BAUDRATE);
 89 |     dx.attach(Serial2, DYNAMIXEL_BAUDRATE);
 90 |     dx.addModel<arduino::Model::X>(1);
 91 |     dx.addModel<arduino::Model::MX>(2);
 92 | 
 93 |     delay(2000);
 94 | 
 95 |     dx.power(1, true);
 96 |     dx.power(2, true);
 97 | 
 98 |     dx.add_bulk_read_target(1, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
 99 |     dx.add_bulk_read_target(2, ADDR_PRO_LED_RED, LEN_PRO_LED_RED);
100 | }
101 | 
102 | void loop()
103 | {
104 |     static bool dir = true;
105 |     int dxl_goal_position[2] = {DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE};         // Goal position
106 |     uint8_t param_goal_position[4];
107 | 
108 |     if (dir)
109 |     {
110 |         param_goal_position[0] = DXL_LOBYTE(DXL_LOWORD(dxl_goal_position[(int)dir]));
111 |         param_goal_position[1] = DXL_HIBYTE(DXL_LOWORD(dxl_goal_position[(int)dir]));
112 |         param_goal_position[2] = DXL_LOBYTE(DXL_HIWORD(dxl_goal_position[(int)dir]));
113 |         param_goal_position[3] = DXL_HIBYTE(DXL_HIWORD(dxl_goal_position[(int)dir]));
114 | 
115 |         uint8_t led_value = 0xFF;
116 | 
117 |         dx.add_bulk_target(1, ADDR_PRO_GOAL_POSITION, LEN_PRO_GOAL_POSITION, param_goal_position);
118 |         dx.add_bulk_target(2, ADDR_PRO_LED_RED, LEN_PRO_RED_LED, param_goal_position, &led_value);
119 |     }
120 |     else
121 |     {
122 |         param_goal_position[0] = DXL_LOBYTE(DXL_LOWORD(dxl_goal_position[(int)dir]));
123 |         param_goal_position[1] = DXL_HIBYTE(DXL_LOWORD(dxl_goal_position[(int)dir]));
124 |         param_goal_position[2] = DXL_LOBYTE(DXL_HIWORD(dxl_goal_position[(int)dir]));
125 |         param_goal_position[3] = DXL_HIBYTE(DXL_HIWORD(dxl_goal_position[(int)dir]));
126 | 
127 |         uint8_t led_value = 0x00;
128 | 
129 |         dx.add_bulk_target(1, ADDR_PRO_GOAL_POSITION, LEN_PRO_GOAL_POSITION, param_goal_position);
130 |         dx.add_bulk_target(2, ADDR_PRO_LED_RED, LEN_PRO_RED_LED, param_goal_position, &led_value);
131 |     }
132 | 
133 |     dx.bulk_write();
134 | 
135 |     delay(500);
136 | 
137 |     dx.bulk_read_request();
138 | 
139 |     if (dx.available(1))
140 |     {
141 |         Serial.print("ID 1 current position = ");
142 |         Serial.println(dx.bulk_read_data(1));
143 |     }
144 |     if (dx.available(2))
145 |     {
146 |         Serial.print("ID 2 LED data = ");
147 |         Serial.println(dx.bulk_read_data(2));
148 |     }
149 | 
150 |     dir = !dir;
151 | }
152 | 


--------------------------------------------------------------------------------
/examples/WIP_indirect_address/indirect_address.ino:
--------------------------------------------------------------------------------
  1 | 
  2 | // Control table address
  3 | // Control table address is different in Dynamixel model
  4 | #define ADDR_PRO_INDIRECTADDRESS_BEGIN      168
  5 | #define ADDR_PRO_TORQUE_ENABLE                  562
  6 | #define ADDR_PRO_LED_RED                        563
  7 | #define ADDR_PRO_GOAL_POSITION                  596
  8 | #define ADDR_PRO_MOVING                         610
  9 | #define ADDR_PRO_PRESENT_POSITION               611
 10 | #define ADDR_PRO_INDIRECTDATA_FOR_WRITE         634
 11 | #define ADDR_PRO_INDIRECTDATA_FOR_READ          639
 12 | 
 13 | // Data Byte Length
 14 | #define LEN_PRO_LED_RED                         1
 15 | #define LEN_PRO_GOAL_POSITION                   4
 16 | #define LEN_PRO_MOVING                          1
 17 | #define LEN_PRO_PRESENT_POSITION                4
 18 | #define LEN_PRO_INDIRECTDATA_FOR_WRITE          5
 19 | #define LEN_PRO_INDIRECTDATA_FOR_READ           5
 20 | 
 21 | #define DXL_MINIMUM_POSITION_VALUE             -150000              // Dynamixel will rotate between this value
 22 | #define DXL_MAXIMUM_POSITION_VALUE              150000              // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
 23 | // TODO: what's this??
 24 | #define DXL_MOVING_STATUS_THRESHOLD             20                  // Dynamixel moving status threshold
 25 | 
 26 | 
 27 | #include <ArduinoDynamixel.h>
 28 | 
 29 | const uint8_t PIN_RTS = 11;
 30 | const uint16_t DYNAMIXEL_BAUDRATE = 57600;
 31 | 
 32 | ArduinoDynamixel dx(PIN_RTS);
 33 | 
 34 | void setup()
 35 | {
 36 |     Serial.begin(115200);
 37 | 
 38 |     delay(2000);
 39 | 
 40 |     Serial2.begin(DYNAMIXEL_BAUDRATE);
 41 |     dx.attach(Serial2, DYNAMIXEL_BAUDRATE);
 42 |     dx.addModel<arduino::Model::MX>
 43 | 
 44 |     delay(2000);
 45 | 
 46 |     // Indirect address would not accessible when the torque is already enabled
 47 |     dx.power(1, false);
 48 | 
 49 | // TODO:
 50 | #if 1
 51 |     dx.indirect.add(1, Reg::GOAL_POS); // automatically append
 52 |     dx.indirect.add(1, Reg::LED_R); // automatically append after
 53 |     dx.indirect.add(1, Reg::PRESENT_POS); // automatically append after
 54 |     dx.indirect.add(1, Reg::MOVING);
 55 | #else
 56 |     dx.setIndirectAddress(1, 0, ADDR_PRO_GOAL_POSITION + 0);
 57 |     dx.setIndirectAddress(1, 1, ADDR_PRO_GOAL_POSITION + 1);
 58 |     dx.setIndirectAddress(1, 2, ADDR_PRO_GOAL_POSITION + 2);
 59 |     dx.setIndirectAddress(1, 3, ADDR_PRO_GOAL_POSITION + 3);
 60 | 
 61 |     dx.setIndirectAddress(1, 4, ADDR_PRO_LED_RED);
 62 | 
 63 |     dx.setIndirectAddress(1, 5, ADDR_PRO_PRESENT_POSITION + 0);
 64 |     dx.setIndirectAddress(1, 6, ADDR_PRO_PRESENT_POSITION + 1);
 65 |     dx.setIndirectAddress(1, 7, ADDR_PRO_PRESENT_POSITION + 2);
 66 |     dx.setIndirectAddress(1, 8, ADDR_PRO_PRESENT_POSITION + 3);
 67 | 
 68 |     dx.setIndirectAddress(1, 9, ADDR_PRO_MOVING);
 69 | #endif
 70 | 
 71 |     dx.power(1, true);
 72 | 
 73 | #if 1
 74 |     // do not use sync_write/read
 75 | #else
 76 |     // TODO: do not use sync_write... maybe longer data cannot be read
 77 |     dx.set_address(ADDR_PRO_INDIRECTADDRESS_BEGIN + 0, LEN_PRO_INDIRECTDATA_FOR_WRITE);
 78 | 
 79 |     // TODO: sync read...
 80 |     dx.set_target(ADDR_PRO_INDIRECTADDRESS_BEGIN + 10, LEN_PRO_INDIRECTDATA_FOR_READ);
 81 |     dx.add_id(1);
 82 | #endif
 83 | }
 84 | 
 85 | void loop()
 86 | {
 87 |     static bool dir = false;
 88 | 
 89 |     int dxl_goal_position[2] = {DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE};
 90 |     uint8_t param_indirect_data_for_write[LEN_PRO_INDIRECTDATA_FOR_WRITE];
 91 |     uint8_t dxl_led_value[2] = {0x00, 0xFF};        // Dynamixel LED value
 92 |     param_indirect_data_for_write[0] = DXL_LOBYTE(DXL_LOWORD(dxl_goal_position[(size_t)dir]));
 93 |     param_indirect_data_for_write[1] = DXL_HIBYTE(DXL_LOWORD(dxl_goal_position[(size_t)dir]));
 94 |     param_indirect_data_for_write[2] = DXL_LOBYTE(DXL_HIWORD(dxl_goal_position[(size_t)dir]));
 95 |     param_indirect_data_for_write[3] = DXL_HIBYTE(DXL_HIWORD(dxl_goal_position[(size_t)dir]));
 96 |     param_indirect_data_for_write[4] = dxl_led_value[(size_t)dir];
 97 | 
 98 |     dx.add(1, param_indirect_data_for_write);
 99 | 
100 |     dx.send();
101 | 
102 |     delay(500);
103 | 
104 |     dx.request();
105 | 
106 |     if (dx.available(1))
107 |     {
108 |         // TODO: sync_read cannot read more than 32bit data...
109 |         // now, 5th byte is ignored...
110 |         // in official example, first read 8bytes, after that, read 2byte...
111 |         Serial.print("current pos = ");
112 |         Serial.println(dx.data(1));
113 | 
114 |     }
115 | 
116 |     dir = !dir;
117 | }


--------------------------------------------------------------------------------
/examples/WIP_sync_read_write/sync_read_write.ino:
--------------------------------------------------------------------------------
 1 | #include <ArduinoDynamixel.h>
 2 | 
 3 | const uint8_t PIN_RTS = 11;
 4 | const uint16_t DYNAMIXEL_BAUDRATE = 57600;
 5 | 
 6 | ArduinoDynamixel dx(PIN_RTS);
 7 | 
 8 | 
 9 | // Control table address
10 | #define ADDR_PRO_TORQUE_ENABLE          562                 // Control table address is different in Dynamixel model
11 | #define ADDR_PRO_GOAL_POSITION          596
12 | #define ADDR_PRO_PRESENT_POSITION       611
13 | 
14 | // Data Byte Length
15 | #define LEN_PRO_GOAL_POSITION           4
16 | #define LEN_PRO_PRESENT_POSITION        4
17 | 
18 | #define DXL_MINIMUM_POSITION_VALUE     -150000              // Dynamixel will rotate between this value
19 | #define DXL_MAXIMUM_POSITION_VALUE      150000              // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
20 | #define DXL_MOVING_STATUS_THRESHOLD     20                  // Dynamixel moving status threshold
21 | 
22 | void setup()
23 | {
24 |   int index = 0;
25 |   int dxl_comm_result = COMM_TX_FAIL;             // Communication result
26 |   bool dxl_addparam_result = false;                // addParam result
27 |   //bool dxl_getdata_result = false;                 // GetParam result
28 |   int32_t dxl1_present_position = 0, dxl2_present_position = 0;              // Present position
29 | 
30 |     Serial.begin(115200);
31 | 
32 |     delay(2000);
33 | 
34 |     Serial2.begin(DYNAMIXEL_BAUDRATE);
35 |     dx.attach(Serial2, DYNAMIXEL_BAUDRATE);
36 | 
37 |     delay(2000);
38 | 
39 |     dx.power(1, true);
40 | 
41 |     dx.set_target(ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
42 |     // TODO: varidic arguments...
43 |     dx.add_id(1);
44 |     dx.add_id(2);
45 | }
46 | 
47 | void loop()
48 | {
49 |     static bool dir = true;
50 | 
51 |     int dxl_goal_position[2] = {DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE};
52 |     uint8_t param_goal_position[4];
53 | 
54 |     // Allocate goal position value into byte array
55 |     param_goal_position[0] = DXL_LOBYTE(DXL_LOWORD(dxl_goal_position[(size_t)dir]));
56 |     param_goal_position[1] = DXL_HIBYTE(DXL_LOWORD(dxl_goal_position[(size_t)dir]));
57 |     param_goal_position[2] = DXL_LOBYTE(DXL_HIWORD(dxl_goal_position[(size_t)dir]));
58 |     param_goal_position[3] = DXL_HIBYTE(DXL_HIWORD(dxl_goal_position[(size_t)dir]));
59 | 
60 |     dx.set_address(ADDR_PRO_GOAL_POSITION, LEN_PRO_GOAL_POSITION);
61 |     dx.add(1, param_goal_position);
62 |     dx.add(2, param_goal_position);
63 | 
64 |     dx.send();
65 | 
66 |     delay(500);
67 | 
68 |     dx.request();
69 | 
70 |     if (dx.availablel(1))
71 |     {
72 |         Serial.print("current pos (ID 1) = "):
73 |         Serial.println(dx.data(1));
74 |     }
75 |     if (dx.availablel(2))
76 |     {
77 |         Serial.print("current pos (ID 2) = "):
78 |         Serial.println(dx.data(2));
79 |     }
80 | 
81 |     dir = !dir;
82 | }
83 | 


--------------------------------------------------------------------------------
/examples/broadcast_ping/broadcast_ping.ino:
--------------------------------------------------------------------------------
 1 | #include <Dynamixel.h>
 2 | 
 3 | #define DYNAMIXEL_SERIAL Serial2 // change as you want
 4 | 
 5 | const uint8_t TARGET_ID = 1;
 6 | const uint8_t PIN_RTS = 11;
 7 | const uint16_t DYNAMIXEL_BAUDRATE = 57600;
 8 | 
 9 | Dynamixel dxl(PIN_RTS);
10 | 
11 | void setup()
12 | {
13 |     Serial.begin(115200);
14 | 
15 |     delay(2000);
16 | 
17 |     DYNAMIXEL_SERIAL.begin(DYNAMIXEL_BAUDRATE);
18 |     dxl.attach(DYNAMIXEL_SERIAL, DYNAMIXEL_BAUDRATE);
19 |     dxl.addModel<DxlModel::X>(TARGET_ID);
20 | 
21 |     delay(2000);
22 | }
23 | 
24 | void loop()
25 | {
26 |     Serial.println("broadcast ping start...");
27 | 
28 |     auto ids = dxl.ping(); // vector<uint8_t>
29 | 
30 |     if (!ids.empty())
31 |     {
32 |         Serial.print("Detected Dynamixel : \n");
33 |         for (int i = 0; i < (int)ids.size(); i++)
34 |         {
35 |             Serial.print("[ID:"); Serial.print(ids.at(i));
36 |             Serial.println("]");
37 |         }
38 |     }
39 |     else
40 |     {
41 |         Serial.println("dynamixel not found");
42 |     }
43 |     delay(2000);
44 | }
45 | 


--------------------------------------------------------------------------------
/examples/factory_reset/factory_reset.ino:
--------------------------------------------------------------------------------
 1 | #include <Dynamixel.h>
 2 | 
 3 | #define DYNAMIXEL_SERIAL Serial2 // change as you want
 4 | 
 5 | const uint8_t TARGET_ID = 1;
 6 | const uint8_t PIN_RTS = 11;
 7 | const uint16_t DYNAMIXEL_BAUDRATE = 57600;
 8 | 
 9 | Dynamixel dxl(PIN_RTS);
10 | 
11 | void setup()
12 | {
13 |     Serial.begin(115200);
14 | 
15 |     delay(2000);
16 | 
17 |     DYNAMIXEL_SERIAL.begin(DYNAMIXEL_BAUDRATE);
18 |     dxl.attach(DYNAMIXEL_SERIAL, DYNAMIXEL_BAUDRATE);
19 |     dxl.addModel<DxlModel::X>(TARGET_ID);
20 | 
21 |     delay(2000);
22 | 
23 |     if (dxl.factoryReset(TARGET_ID))
24 |         Serial.println("factory reset succeed");
25 |     else
26 |         Serial.println("factory reset failed");
27 | }
28 | 
29 | void loop()
30 | {
31 | }


--------------------------------------------------------------------------------
/examples/move/move.ino:
--------------------------------------------------------------------------------
 1 | #include <Dynamixel.h>
 2 | 
 3 | #define DYNAMIXEL_SERIAL Serial2 // change as you want
 4 | 
 5 | const uint8_t TARGET_ID = 1;
 6 | const uint8_t PIN_RTS = 11;
 7 | const uint16_t DYNAMIXEL_BAUDRATE = 57600;
 8 | 
 9 | Dynamixel dxl(PIN_RTS);
10 | 
11 | int dxl_goal_position[2];
12 | 
13 | void setup()
14 | {
15 |     Serial.begin(115200);
16 | 
17 |     delay(2000);
18 | 
19 |     DYNAMIXEL_SERIAL.begin(DYNAMIXEL_BAUDRATE);
20 |     dxl.attach(DYNAMIXEL_SERIAL, DYNAMIXEL_BAUDRATE);
21 |     dxl.addModel<DxlModel::X>(TARGET_ID);
22 | 
23 |     delay(2000);
24 | 
25 |     dxl.torqueEnable(TARGET_ID, false);
26 | 
27 |     Serial.print("min pos = ");
28 |     Serial.println(dxl.minPositionLimit(TARGET_ID));
29 |     Serial.print("max pos = ");
30 |     Serial.println(dxl.maxPositionLimit(TARGET_ID));
31 | 
32 |     dxl.minPositionLimit(TARGET_ID, 1400);
33 |     dxl.verbose(TARGET_ID);
34 |     dxl.maxPositionLimit(TARGET_ID, 1900);
35 |     dxl.verbose(TARGET_ID);
36 | 
37 |     dxl_goal_position[0] = dxl.minPositionLimit(TARGET_ID);
38 |     dxl_goal_position[1] = dxl.maxPositionLimit(TARGET_ID);
39 | 
40 |     dxl.velocityLimit(TARGET_ID, 30000);
41 | 
42 |     Serial.print("min pos = ");
43 |     Serial.println(dxl.minPositionLimit(TARGET_ID));
44 |     Serial.print("max pos = ");
45 |     Serial.println(dxl.maxPositionLimit(TARGET_ID));
46 | 
47 |     dxl.torqueEnable(TARGET_ID, true);
48 | }
49 | 
50 | void loop()
51 | {
52 |     static bool dir = true;
53 | 
54 |     dxl.goalPosition(TARGET_ID, dxl_goal_position[(size_t)dir]);
55 |     dxl.verbose(TARGET_ID);
56 | 
57 |     delay(3000);
58 | 
59 |     Serial.print("current pos = ");
60 |     Serial.println(dxl.presentPosition(TARGET_ID));
61 |     dxl.verbose(TARGET_ID);
62 | 
63 |     dir = !dir;
64 | }
65 | 


--------------------------------------------------------------------------------
/examples/ping/ping.ino:
--------------------------------------------------------------------------------
 1 | #include <Dynamixel.h>
 2 | 
 3 | #define DYNAMIXEL_SERIAL Serial2 // change as you want
 4 | 
 5 | const uint8_t TARGET_ID = 1;
 6 | const uint8_t PIN_RTS = 11;
 7 | const uint16_t DYNAMIXEL_BAUDRATE = 57600;
 8 | 
 9 | Dynamixel dxl(PIN_RTS);
10 | 
11 | void setup()
12 | {
13 |     Serial.begin(115200);
14 | 
15 |     delay(2000);
16 | 
17 |     DYNAMIXEL_SERIAL.begin(DYNAMIXEL_BAUDRATE);
18 |     dxl.attach(DYNAMIXEL_SERIAL, DYNAMIXEL_BAUDRATE);
19 |     dxl.addModel<DxlModel::X>(TARGET_ID);
20 | 
21 |     delay(2000);
22 | }
23 | 
24 | void loop()
25 | {
26 |     if (dxl.ping(TARGET_ID))
27 |     {
28 |         Serial.print("[ID:"); Serial.print(TARGET_ID);
29 |         Serial.print("] ping Succeeded. Dynamixel model number : ");
30 |         Serial.println(dxl.modelNumber(TARGET_ID), HEX);
31 |     }
32 |     else
33 |     {
34 |         Serial.print("[ID:"); Serial.print(TARGET_ID);
35 |         Serial.print("] ping Failed.");
36 |         dxl.verbose(TARGET_ID);
37 |     }
38 |     delay(1000);
39 | }
40 | 


--------------------------------------------------------------------------------
/examples/reboot/reboot.ino:
--------------------------------------------------------------------------------
 1 | #include <Dynamixel.h>
 2 | 
 3 | #define DYNAMIXEL_SERIAL Serial2 // change as you want
 4 | 
 5 | const uint8_t TARGET_ID = 1;
 6 | const uint8_t PIN_RTS = 11;
 7 | const uint16_t DYNAMIXEL_BAUDRATE = 57600;
 8 | 
 9 | Dynamixel dxl(PIN_RTS);
10 | 
11 | void setup()
12 | {
13 |     Serial.begin(115200);
14 | 
15 |     delay(2000);
16 | 
17 |     DYNAMIXEL_SERIAL.begin(DYNAMIXEL_BAUDRATE);
18 |     dxl.attach(DYNAMIXEL_SERIAL, DYNAMIXEL_BAUDRATE);
19 |     dxl.addModel<DxlModel::X>(TARGET_ID);
20 | 
21 |     delay(2000);
22 | }
23 | 
24 | void loop()
25 | {
26 |     Serial.println("start reboot... see LED flickering");
27 |     dxl.reboot(TARGET_ID);
28 |     delay(5000);
29 | }
30 | 


--------------------------------------------------------------------------------
/examples/speed/speed.ino:
--------------------------------------------------------------------------------
  1 | #include <Dynamixel.h>
  2 | 
  3 | #define DYNAMIXEL_SERIAL Serial2 // change as you want
  4 | const uint8_t TARGET_ID = 1;
  5 | const uint8_t PIN_RTS = 11;
  6 | const uint16_t DYNAMIXEL_BAUDRATE = 57600;
  7 | 
  8 | Dynamixel dxl(PIN_RTS);
  9 | 
 10 | int goal_pos[2];
 11 | 
 12 | // TODO:
 13 | using namespace arduino;
 14 | 
 15 | void setup()
 16 | {
 17 |     Serial.begin(115200);
 18 | 
 19 |     delay(2000);
 20 | 
 21 |     DYNAMIXEL_SERIAL.begin(DYNAMIXEL_BAUDRATE);
 22 |     dxl.attach(DYNAMIXEL_SERIAL, DYNAMIXEL_BAUDRATE);
 23 |     dxl.addModel<DxlModel::X>(TARGET_ID);
 24 | 
 25 |     delay(2000);
 26 | 
 27 |     dxl.torqueEnable(TARGET_ID, false);
 28 | 
 29 |     Serial.print("op mode = ");
 30 |     Serial.println(dxl.operatingMode(TARGET_ID));
 31 |     Serial.print("drvie mode = ");
 32 |     Serial.println(dxl.driveMode(TARGET_ID));
 33 |     Serial.print("min pos = ");
 34 |     Serial.println(dxl.minPositionLimit(TARGET_ID));
 35 |     Serial.print("max pos = ");
 36 |     Serial.println(dxl.maxPositionLimit(TARGET_ID));
 37 |     Serial.print("acc limit = ");
 38 |     Serial.println(dxl.accelerationLimit(TARGET_ID));
 39 |     Serial.print("vel limit = ");
 40 |     Serial.println(dxl.velocityLimit(TARGET_ID));
 41 |     Serial.print("vel goal = ");
 42 |     Serial.println(dxl.goalVelocity(TARGET_ID));
 43 |     Serial.print("profile velocity = ");
 44 |     Serial.println(dxl.profileVelocity(TARGET_ID));
 45 |     Serial.print("profile acceleration= ");
 46 |     Serial.println(dxl.profileAcceleration(TARGET_ID));
 47 | 
 48 | 
 49 |     dxl.minPositionLimit(TARGET_ID, 0);
 50 |     dxl.maxPositionLimit(TARGET_ID, 4095);
 51 |     dxl.profileVelocity(TARGET_ID, 171); // Max is 171, 0.229 * 170.305676856 = 39 [rpm] @12V
 52 |     dxl.profileAcceleration(TARGET_ID, 0); // 0: inifinity
 53 | 
 54 |     goal_pos[0] = dxl.minPositionLimit(TARGET_ID);
 55 |     goal_pos[1] = dxl.maxPositionLimit(TARGET_ID);
 56 | 
 57 |     Serial.print("op mode = ");
 58 |     Serial.println(dxl.operatingMode(TARGET_ID));
 59 |     Serial.print("drvie mode = ");
 60 |     Serial.println(dxl.driveMode(TARGET_ID));
 61 |     Serial.print("min pos = ");
 62 |     Serial.println(dxl.minPositionLimit(TARGET_ID));
 63 |     Serial.print("max pos = ");
 64 |     Serial.println(dxl.maxPositionLimit(TARGET_ID));
 65 |     Serial.print("acc limit = ");
 66 |     Serial.println(dxl.accelerationLimit(TARGET_ID));
 67 |     Serial.print("vel limit = ");
 68 |     Serial.println(dxl.velocityLimit(TARGET_ID));
 69 |     Serial.print("vel goal = ");
 70 |     Serial.println(dxl.goalVelocity(TARGET_ID));
 71 |     Serial.print("profile velocity = ");
 72 |     Serial.println(dxl.profileVelocity(TARGET_ID));
 73 |     Serial.print("profile acceleration= ");
 74 |     Serial.println(dxl.profileAcceleration(TARGET_ID));
 75 | 
 76 |     dxl.torqueEnable(TARGET_ID, true);
 77 | }
 78 | 
 79 | void loop()
 80 | {
 81 |     static bool dir = true;
 82 | 
 83 | #if 0
 84 |     dxl.goalPosition(TARGET_ID, goal_pos[(size_t)dir]);
 85 |     dxl.verbose(TARGET_ID);
 86 | 
 87 |     delay(3000);
 88 | 
 89 |     Serial.print("current pos = ");
 90 |     Serial.println(dxl.presentPosition(TARGET_ID));
 91 |     dxl.verbose(TARGET_ID);
 92 | 
 93 |     dir = !dir;
 94 | #else
 95 |     static uint16_t pos = 0;
 96 |     if (dir)
 97 |     {
 98 |         if (++pos > 4095) dir = !dir;
 99 |     }
100 |     else
101 |     {
102 |         if (--pos < 1) dir = !dir;
103 |     }
104 |     dxl.goalPosition(TARGET_ID, pos);
105 |     dxl.verbose(TARGET_ID);
106 |     delay(20);
107 |     Serial.println(pos);
108 | #endif
109 | }
110 | 


--------------------------------------------------------------------------------
/examples/two_motors/two_motors.ino:
--------------------------------------------------------------------------------
  1 | #include <Dynamixel.h>
  2 | 
  3 | #define DYNAMIXEL_SERIAL Serial2 // change as you want
  4 | 
  5 | const uint8_t TARGET_ID_1 = 1; // daisy-chained first dynamixel
  6 | const uint8_t TARGET_ID_2 = 2; // daisy-chained second dynamixel
  7 | const uint8_t PIN_RTS = 11;
  8 | const uint16_t DYNAMIXEL_BAUDRATE = 57600;
  9 | 
 10 | Dynamixel dxl(PIN_RTS);
 11 | 
 12 | int dxl_goal_position_1[2];
 13 | int dxl_goal_position_2[2];
 14 | 
 15 | // TODO:
 16 | using namespace arduino;
 17 | 
 18 | void setup()
 19 | {
 20 |     Serial.begin(115200);
 21 | 
 22 |     delay(2000);
 23 | 
 24 |     DYNAMIXEL_SERIAL.begin(DYNAMIXEL_BAUDRATE);
 25 |     dxl.attach(DYNAMIXEL_SERIAL, DYNAMIXEL_BAUDRATE);
 26 |     dxl.addModel<DxlModel::X>(TARGET_ID_1);
 27 |     dxl.addModel<DxlModel::MX>(TARGET_ID_2);
 28 | 
 29 |     delay(2000);
 30 | 
 31 |     dxl.torqueEnable(TARGET_ID_1, false);
 32 |     dxl.torqueEnable(TARGET_ID_2, false);
 33 | 
 34 |     Serial.println("ID 1 : ");
 35 |     Serial.print("min pos = ");
 36 |     Serial.println(dxl.minPositionLimit(TARGET_ID_1));
 37 |     Serial.print("max pos = ");
 38 |     Serial.println(dxl.maxPositionLimit(TARGET_ID_1));
 39 | 
 40 |     Serial.println("ID 2 : ");
 41 |     Serial.print("min pos = ");
 42 |     Serial.println(dxl.minPositionLimit(TARGET_ID_1));
 43 |     Serial.print("max pos = ");
 44 |     Serial.println(dxl.maxPositionLimit(TARGET_ID_1));
 45 | 
 46 |     dxl.minPositionLimit(TARGET_ID_1, 1400);
 47 |     dxl.verbose(TARGET_ID_1);
 48 |     dxl.maxPositionLimit(TARGET_ID_1, 1900);
 49 |     dxl.verbose(TARGET_ID_1);
 50 | 
 51 |     dxl.minPositionLimit(TARGET_ID_2, 1400);
 52 |     dxl.verbose(TARGET_ID_2);
 53 |     dxl.maxPositionLimit(TARGET_ID_2, 1900);
 54 |     dxl.verbose(TARGET_ID_2);
 55 | 
 56 |     dxl_goal_position_1[0] = dxl.minPositionLimit(TARGET_ID_1);
 57 |     dxl_goal_position_1[1] = dxl.maxPositionLimit(TARGET_ID_1);
 58 | 
 59 |     dxl_goal_position_2[0] = dxl.minPositionLimit(TARGET_ID_2);
 60 |     dxl_goal_position_2[1] = dxl.maxPositionLimit(TARGET_ID_2);
 61 | 
 62 |     dxl.velocityLimit(TARGET_ID_1, 30000);
 63 |     dxl.velocityLimit(TARGET_ID_2, 30000);
 64 | 
 65 |     Serial.println("ID 1 : ");
 66 |     Serial.print("min pos = ");
 67 |     Serial.println(dxl.minPositionLimit(TARGET_ID_1));
 68 |     Serial.print("max pos = ");
 69 |     Serial.println(dxl.maxPositionLimit(TARGET_ID_1));
 70 | 
 71 |     Serial.println("ID 2 : ");
 72 |     Serial.print("min pos = ");
 73 |     Serial.println(dxl.minPositionLimit(TARGET_ID_1));
 74 |     Serial.print("max pos = ");
 75 |     Serial.println(dxl.maxPositionLimit(TARGET_ID_1));
 76 | 
 77 |     dxl.torqueEnable(TARGET_ID_1, true);
 78 |     dxl.torqueEnable(TARGET_ID_2, true);
 79 | }
 80 | 
 81 | void loop()
 82 | {
 83 |     static bool dir = true;
 84 | 
 85 |     dxl.goalPosition(TARGET_ID_1, dxl_goal_position_1[(size_t)dir]);
 86 |     dxl.goalPosition(TARGET_ID_2, dxl_goal_position_2[(size_t)dir]);
 87 |     dxl.verbose(TARGET_ID_1);
 88 |     dxl.verbose(TARGET_ID_2);
 89 | 
 90 |     delay(3000);
 91 | 
 92 |     Serial.print("current pos (ID 1) = ");
 93 |     Serial.println(dxl.presentPosition(TARGET_ID_1));
 94 |     dxl.verbose(TARGET_ID_1);
 95 | 
 96 |     Serial.print("current pos (ID 2) = ");
 97 |     Serial.println(dxl.presentPosition(TARGET_ID_2));
 98 |     dxl.verbose(TARGET_ID_2);
 99 | 
100 |     dir = !dir;
101 | }
102 | 


--------------------------------------------------------------------------------
/library.json:
--------------------------------------------------------------------------------
 1 | {
 2 | 	"name": "Dynamixel",
 3 | 	"keywords": "dynamixel,motor,servo,serial,rs485",
 4 | 	"description": "Arduino library for Dynamixel",
 5 | 	"repository": {
 6 | 		"type": "git",
 7 | 		"url": "https://github.com/hideakitai/Dynamixel.git"
 8 | 	},
 9 | 	"authors": {
10 | 		"name": "Hideaki Tai",
11 | 		"url": "https://github.com/hideakitai",
12 | 		"maintainer": true
13 | 	},
14 | 	"version": "0.3.1",
15 | 	"license": "MIT",
16 | 	"frameworks": "arduino",
17 | 	"platforms": "*",
18 | 	"dependencies": {
19 | 		"hideakitai/ArxContainer": ">=0.6.0",
20 | 		"hideakitai/ArxSmartPtr": "*",
21 | 		"hideakitai/ArxTypeTraits": "*"
22 | 	}
23 | }
24 | 


--------------------------------------------------------------------------------
/library.properties:
--------------------------------------------------------------------------------
 1 | name=Dynamixel
 2 | version=0.3.1
 3 | author=hideakitai
 4 | maintainer=hideakitai
 5 | sentence=Arduino library for Dynamixel
 6 | paragraph=Arduino library for Dynamixel
 7 | category=Device Control
 8 | url=https://github.com/hideakitai/Dynamixel
 9 | architectures=*
10 | depends=ArxContainer (>=0.6.0), ArxSmartPtr, ArxTypeTraits
11 | 


--------------------------------------------------------------------------------