YARP
Yet Another Robot Platform
DynamixelAX12FtdiDriver.h
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-FileCopyrightText: 2010 Ze Ji
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7 /*
8 * YARP Driver for Dynamixel AX-12, controlled using USB to Dynamixel Adapter
9 * The default IDs for the motors are for Kaspar robot. As different robots would have different settings,
10 * it is necessary to define the IDs, this can be done via a configuration file. In this example, check the
11 * file sensorindex.cfg. The format is:
12 * SENSORINDEX 16 101 116 132 102 117 133 103 118 134 107 119 135 106 104 105 108
13 * This needs to be done by running configure(), before opening the device
14 *
15 * As it is using libftdi library, it is not possible to identify a device by specifying the serial port number, such as
16 * /dev/ttyUSB0
17 * Instead, this driver requires precise information of the ftdi device, such as the dynamixel usb manufacture id, serial number, etc. See class FtdiDeviceSettings for details.
18 *
19 * The motor does not support Torque control, but provide torque feedback. Therefore, several functions
20 * have been implemented for such purpose, though not very rigorous.
21 *
22 * Author: Ze Ji - z.ji@herts.ac.uk (University of Hertfordshire, UK), 2010
23 * as part of the RoboSkin project (EU FP7).
24 * Part of the work is initially from Sven Magg, 2009
25 */
26
27#ifndef DynamixelAX12Driverh
28#define DynamixelAX12Driverh
29
30//#define __DEBUG__
31
34
35#include <cstdio>
36#include <cstdlib>
37#include <cmath>
38#include <cstring>
39#include <ftdi.h>
40//#include <libftdi/ftdi.h>
41#include <usb.h>
42#include <iostream>
43#include <yarp/os/Time.h>
44
45#include <mutex>
46
47#define MOTION_COMPLETION_TOLERANCE 3
48
51#define INST_PING 0x01
52#define INST_READ 0x02
53#define INST_WRITE 0x03
54#define INST_REGWRITE 0x04
55#define INST_ACTION 0x05
56#define INST_RESET 0x06
57#define INST_SYNCWRITE 0x83
58
60#define AX12_BROADCAST_ID 0xFE
61
65#define CT_MODEL_NUMBER 00
66#define CT_FIRMWARE_VERSION 2
67#define CT_RETURN_DELAY_TIME 5
68#define CT_CW_ANGLE_LIMIT 06
69#define CT_CWW_ANGLE_LIMIT 08
70#define CT_LIMIT_TEMPERATURE_H 11
71#define CT_LIMIT_VOLTAGE_L 12
72#define CT_LIMIT_VOLTAGE_H 13
73#define CT_MAX_TORQUE 14
74#define CT_STATUS_RETURN_LEVEL 16
75
78#define CT_TORQUE_ENABLE 24
79#define CT_CW_COMP_MARGIN 26
80#define CT_CWW_COMP_MARGIN 27
81#define CT_CW_COMP_SLOPE 28
82#define CT_CWW_COMP_SLOPE 29
83#define CT_GOAL_POSITION 30
84#define CT_MOVING_SPEED 32
85#define CT_TORQUE_LIMIT 34
86#define CT_PRESENT_POSITION 0x24
87#define CT_PRESENT_SPEED 0x26
88#define CT_PRESENT_LOAD 0x28
89#define CT_PRESENT_VOLTAGE 42
90#define CT_PRESENT_TEMPERATURE 43
91#define CT_REG_INSTRUCTION 44
92#define CT_MOVING 46
93#define CT_LOCK 47
94#define CT_PUNCH 48
95
104 OK
106
107using namespace yarp::os;
108using namespace yarp::dev;
109
116{
117public:
118 int vendor; //0x0403 normally. Can be found by lsusb on linux
119 int product; //0x6001 Can be found by lsusb on linux
120 char description[100]; // Contains the description
121 char manufacturer[100]; // Contains manufacturer
122 char serial[100]; // used mainly, as this seems unique
123 int baudrate; // if ((retCode = ftdi_set_baudrate(&ftdic, config2.SerialParams.baudrate)) != 0)
125 // int bits; // if (ftdi_set_line_property(&ftdic, BITS_8, STOP_BIT_1, NONE) == -1)
126 // int parity;
127 // int sbit;
128 int flowctrol; // flow control to use. should be SIO_DISABLE_FLOW_CTRL, SIO_RTS_CTS_HS, SIO_DTR_DSR_HS or SIO_XON_XOFF_HS
129
130 unsigned int write_chunksize; // e.g. ftdi_write_data_set_chunksize(&ftdic, 3);
131 unsigned int read_chunksize; // ftdi_read_data_set_chunksize(&ftdic, 256);
132};
133
144{
145private:
147 struct ftdi_context ftdic;
149 bool deviceOpen;
150
151 ErrorCode checkAnswerPacket(unsigned char* packet, const char*& message);
152
153 std::mutex mutex;
154
155 unsigned char *jointNumbers;
156
157 int numOfAxes; // default = 16
158
159public:
160
165
169 virtual ~DynamixelAX12FtdiDriver();
170
177 bool open(yarp::os::Searchable& config) override;
178
184 bool close() override;
185
192 bool configure(yarp::os::Searchable& config) override;
193
203 virtual int sendCommand(unsigned char id, unsigned char inst[], int size, unsigned char ret[], int &retSize);
204
212 virtual int readParameter(unsigned char id, unsigned char param);
213
214 bool getAxes(int *ax) override;
215
219 bool positionMove(int j, double ref) override;
220 bool positionMove(const double *refs) override;
221 bool relativeMove(int j, double delta) override;
222 bool relativeMove(const double *deltas) override;
223 bool checkMotionDone(int j, bool *flag) override;
224 bool checkMotionDone(bool *flag) override;
225 bool setRefSpeed(int j, double sp) override;
226 bool setRefSpeeds(const double *spds) override;
227 bool setRefAcceleration(int j, double acc) override;
228 bool setRefAccelerations(const double *accs) override;
229 bool getRefSpeed(int j, double *ref) override;
230 bool getRefSpeeds(double *spds) override;
231 bool getRefAcceleration(int j, double *acc) override;
232 bool getRefAccelerations(double *accs) override;
233 bool stop(int j) override;
234 bool stop() override;
235
236 bool getRefTorques(double *t) override;
237 bool getRefTorque(int j, double *t) override;
238 bool setTorques(const double *t);
239 bool setTorque(int j, double t);
240 bool setTorquePid(int j, const Pid &pid);
241 bool getTorque(int j, double *t) override;
242 bool getTorques(double *t) override;
243 bool setTorquePids(const Pid *pids);
244 bool setTorqueErrorLimit(int j, double limit);
245 bool setTorqueErrorLimits(const double *limits);
246 bool getTorqueError(int j, double *err);
247 bool getTorqueErrors(double *errs);
248 bool getTorquePidOutput(int j, double *out);
249 bool getTorquePidOutputs(double *outs);
250 bool getTorquePid(int j, Pid *pid);
251 bool getTorquePids(Pid *pids);
252 bool getTorqueErrorLimit(int j, double *limit);
253 bool getTorqueErrorLimits(double *limits);
254 bool resetTorquePid(int j);
255 bool disableTorquePid(int j);
256 bool enableTorquePid(int j);
257 bool setTorqueOffset(int j, double v);
258
259 bool getBemfParam(int j, double *bemf);
260 bool setBemfParam(int j, double bemf);
261
262 bool resetEncoder(int j) override;
263 bool resetEncoders() override;
264
265 bool setEncoder(int j, double val) override;
266 bool setEncoders(const double *vals) override;
270 bool getEncoder(int j, double *v) override;
271 bool getEncoders(double *encs) override;
272 bool getEncoderSpeed(int j, double *sp) override;
273 bool getEncoderSpeeds(double *spds) override;
274 bool getEncoderAcceleration(int j, double *spds) override;
275 bool getEncoderAccelerations(double *accs) override;
276
278 bool setRefTorques(const double* t) override;
279
280 bool setRefTorque(int j, double t) override;
281 bool getTorqueRange(int j, double* min, double* max) override;
282 bool getTorqueRanges(double* min, double* max) override;
283
284 // multiple joint version
285 bool positionMove(const int n_joint, const int *joints, const double *refs) override;
286 bool relativeMove(const int n_joint, const int *joints, const double *deltas) override;
287 bool checkMotionDone(const int n_joint, const int *joints, bool *flag) override;
288 bool setRefSpeeds(const int n_joint, const int *joints, const double *spds) override;
289 bool setRefAccelerations(const int n_joint, const int *joints, const double *accs) override;
290 bool getRefSpeeds(const int n_joint, const int *joints, double *spds) override;
291 bool getRefAccelerations(const int n_joint, const int *joints, double *accs) override;
292 bool stop(const int n_joint, const int *joints) override;
293
294private:
295
296 double *positions;
297 double *speeds;
298
299 struct ftdi_device_list *devlist;
300 struct ftdi_device_list *curdev;
302 int *torques;
303
304 int normalisePosition(double position);
305 int normaliseSpeed(double speed);
306
312 bool initMotorIndex(yarp::os::Bottle *sensorIndex);
313
314 int syncSendCommand(unsigned char id, unsigned char inst[], int size, unsigned char ret[], int &retSize);
315
316 //bool lock; // lock to ensure sendcommand can be only accessed by one request at one time.
317};
318
319//#undef __DEBUG__
320
321#endif
define control board standard interfaces
@ OVERHEATING_ERROR
@ INSTRUCTION_ERROR
float t
bool ret
dynamixelAX12Ftdi: Documentation to be added
bool setRefAccelerations(const double *accs) override
not implemented
bool getEncoderSpeed(int j, double *sp) override
Read the istantaneous speed of an axis.
bool setTorquePid(int j, const Pid &pid)
bool getTorquePid(int j, Pid *pid)
bool setTorqueOffset(int j, double v)
bool close() override
Close device Closes the device and shuts down connection.
bool getTorque(int j, double *t) override
Get the value of the torque on a given joint (this is the feedback if you have a torque sensor).
bool setTorqueErrorLimits(const double *limits)
virtual int sendCommand(unsigned char id, unsigned char inst[], int size, unsigned char ret[], int &retSize)
Send instruction to device Send an instruction to a device of given ID.
bool setTorquePids(const Pid *pids)
bool getBemfParam(int j, double *bemf)
virtual int readParameter(unsigned char id, unsigned char param)
Read parameter from motor Requests the value of a parameter from motor.
bool resetEncoder(int j) override
Reset encoder, single joint.
bool getTorquePidOutputs(double *outs)
bool setTorques(const double *t)
bool getRefTorque(int j, double *t) override
Get the reference value of the torque for a given joint.
bool setRefAcceleration(int j, double acc) override
not implemented
bool setEncoders(const double *vals) override
Set the value of all encoders.
bool getEncoder(int j, double *v) override
Get the angle of servo.
bool configure(yarp::os::Searchable &config) override
Configure device online Configures parts of the device that can be configures online.
bool getTorqueError(int j, double *err)
bool setRefTorque(int j, double t) override
Set the reference value of the torque for a given joint.
bool getRefSpeeds(double *spds) override
Get reference speed of all joints.
bool getEncoderAccelerations(double *accs) override
Read the instantaneous acceleration of all axes.
bool setEncoder(int j, double val) override
Set the value of the encoder for a given joint.
bool getTorqueErrorLimits(double *limits)
bool setRefTorques(const double *t) override
Set the reference value of the torque for all joints.
bool getEncoders(double *encs) override
Read the position of all axes.
bool getRefSpeed(int j, double *ref) override
Get reference speed for a joint.
bool checkMotionDone(int j, bool *flag) override
Check if the current trajectory is terminated.
bool getEncoderSpeeds(double *spds) override
Read the instantaneous speed of all axes.
bool open(yarp::os::Searchable &config) override
Open device Opens and configures the device.
DynamixelAX12FtdiDriver()
Constructor Initializes handle but doesn't open channel yet.
virtual ~DynamixelAX12FtdiDriver()
Destructor Closes connection and destroys device object.
bool getTorqueRange(int j, double *min, double *max) override
Get the full scale of the torque sensor of a given joint.
bool getAxes(int *ax) override
Get the number of controlled axes.
bool getRefAccelerations(double *accs) override
not implemented
bool resetEncoders() override
Reset encoders.
bool setTorqueErrorLimit(int j, double limit)
bool setBemfParam(int j, double bemf)
bool getEncoderAcceleration(int j, double *spds) override
Read the instantaneous acceleration of an axis.
bool setRefSpeeds(const double *spds) override
Set reference speed on all joints.
bool getTorques(double *t) override
Get the value of the torque for all joints (this is the feedback if you have torque sensors).
bool stop() override
Stop motion, multiple joints.
bool positionMove(int j, double ref) override
bool getRefTorques(double *t) override
Get the reference value of the torque for all joints.
bool getTorqueErrorLimit(int j, double *limit)
bool getRefAcceleration(int j, double *acc) override
not implemented
bool getTorqueRanges(double *min, double *max) override
Get the full scale of the torque sensors of all joints.
bool getTorquePidOutput(int j, double *out)
bool relativeMove(int j, double delta) override
Set relative position.
bool setRefSpeed(int j, double sp) override
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
FtdiDeviceSettings contains information to identify specific device Such a device can contain informa...
Interface implemented by all device drivers.
Definition: DeviceDriver.h:30
Control board, encoder interface.
Definition: IEncoders.h:116
Interface for a generic control board device implementing position control.
Interface for control boards implementing torque control.
virtual bool setRefTorques(const double *t)=0
Set the reference value of the torque for all joints.
Contains the parameters for a PID.
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:64
A base class for nested structures that can be searched.
Definition: Searchable.h:56
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.