YARP
Yet Another Robot Platform
DynamixelAX12FtdiDriver.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
3  * Copyright (C) 2010 Ze Ji
4  * All rights reserved.
5  *
6  * This software may be modified and distributed under the terms of the
7  * BSD-3-Clause license. See the accompanying LICENSE file for details.
8  */
9 
10  /*
11  * Yarp Driver for Dynamixel AX-12, controlled using USB to Dynamixel Adapter
12  * The default IDs for the motors are for Kaspar robot. As different robots would have different settings,
13  * it is necessary to define the IDs, this can be done via a configuration file. In this example, check the
14  * file sensorindex.cfg. The format is:
15  * SENSORINDEX 16 101 116 132 102 117 133 103 118 134 107 119 135 106 104 105 108
16  * This needs to be done by running configure(), before opening the device
17  *
18  * As it is using libftdi library, it is not possible to identify a device by specifying the serial port number, such as
19  * /dev/ttyUSB0
20  * 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.
21  *
22  * The motor does not support Torque control, but provide torque feedback. Therefore, several functions
23  * have been implemented for such purpose, though not very rigorous.
24  *
25  * Author: Ze Ji - z.ji@herts.ac.uk (University of Hertfordshire, UK), 2010
26  * as part of the RoboSkin project (EU FP7).
27  * Part of the work is initially from Sven Magg, 2009
28  */
29 
30 #ifndef DynamixelAX12Driverh
31 #define DynamixelAX12Driverh
32 
33 //#define __DEBUG__
34 
35 #include <yarp/dev/DeviceDriver.h>
36 #include <yarp/dev/ControlBoardInterfaces.h>
37 
38 #include <cstdio>
39 #include <cstdlib>
40 #include <cmath>
41 #include <cstring>
42 #include <ftdi.h>
43 //#include <libftdi/ftdi.h>
44 #include <usb.h>
45 #include <iostream>
46 #include <yarp/os/Time.h>
47 
48 #include <mutex>
49 
50 #define MOTION_COMPLETION_TOLERANCE 3
51 
54 #define INST_PING 0x01
55 #define INST_READ 0x02
56 #define INST_WRITE 0x03
57 #define INST_REGWRITE 0x04
58 #define INST_ACTION 0x05
59 #define INST_RESET 0x06
60 #define INST_SYNCWRITE 0x83
61 
63 #define AX12_BROADCAST_ID 0xFE
64 
68 #define CT_MODEL_NUMBER 00
69 #define CT_FIRMWARE_VERSION 2
70 #define CT_RETURN_DELAY_TIME 5
71 #define CT_CW_ANGLE_LIMIT 06
72 #define CT_CWW_ANGLE_LIMIT 08
73 #define CT_LIMIT_TEMPERATURE_H 11
74 #define CT_LIMIT_VOLTAGE_L 12
75 #define CT_LIMIT_VOLTAGE_H 13
76 #define CT_MAX_TORQUE 14
77 #define CT_STATUS_RETURN_LEVEL 16
78 
81 #define CT_TORQUE_ENABLE 24
82 #define CT_CW_COMP_MARGIN 26
83 #define CT_CWW_COMP_MARGIN 27
84 #define CT_CW_COMP_SLOPE 28
85 #define CT_CWW_COMP_SLOPE 29
86 #define CT_GOAL_POSITION 30
87 #define CT_MOVING_SPEED 32
88 #define CT_TORQUE_LIMIT 34
89 #define CT_PRESENT_POSITION 0x24
90 #define CT_PRESENT_SPEED 0x26
91 #define CT_PRESENT_LOAD 0x28
92 #define CT_PRESENT_VOLTAGE 42
93 #define CT_PRESENT_TEMPERATURE 43
94 #define CT_REG_INSTRUCTION 44
95 #define CT_MOVING 46
96 #define CT_LOCK 47
97 #define CT_PUNCH 48
98 
99 enum ErrorCode {
100  VOLTAGE_ERROR,
101  ANGLE_ERROR,
102  OVERHEATING_ERROR,
103  RANGE_ERROR,
104  CHECKSUM_ERROR,
105  OVERLOAD_ERROR,
106  INSTRUCTION_ERROR,
107  OK
108 };
109 
110 using namespace yarp::os;
111 using namespace yarp::dev;
112 
118 class FtdiDeviceSettings
119 {
120 public:
121  int vendor; //0x0403 normally. Can be found by lsusb on linux
122  int product; //0x6001 Can be found by lsusb on linux
123  char description[100]; // Contains the description
124  char manufacturer[100]; // Contains manufacturer
125  char serial[100]; // used mainly, as this seems unique
126  int baudrate; // if ((retCode = ftdi_set_baudrate(&ftdic, config2.SerialParams.baudrate)) != 0)
127  int readtimeout;
128  // int bits; // if (ftdi_set_line_property(&ftdic, BITS_8, STOP_BIT_1, NONE) == -1)
129  // int parity;
130  // int sbit;
131  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
132 
133  unsigned int write_chunksize; // e.g. ftdi_write_data_set_chunksize(&ftdic, 3);
134  unsigned int read_chunksize; // ftdi_read_data_set_chunksize(&ftdic, 256);
135 };
136 
142 class DynamixelAX12FtdiDriver :
143  public yarp::dev::DeviceDriver,
144  public yarp::dev::IPositionControl,
145  public yarp::dev::ITorqueControl,
146  public yarp::dev::IEncoders
147 {
148 private:
150  struct ftdi_context ftdic;
152  bool deviceOpen;
153 
154  ErrorCode checkAnswerPacket(unsigned char* packet, const char*& message);
155 
156  std::mutex mutex;
157 
158  unsigned char *jointNumbers;
159 
160  int numOfAxes; // default = 16
161 
162 public:
163 
167  DynamixelAX12FtdiDriver();
168 
172  virtual ~DynamixelAX12FtdiDriver();
173 
180  bool open(yarp::os::Searchable& config) override;
181 
187  bool close() override;
188 
195  bool configure(yarp::os::Searchable& config) override;
196 
206  virtual int sendCommand(unsigned char id, unsigned char inst[], int size, unsigned char ret[], int &retSize);
207 
215  virtual int readParameter(unsigned char id, unsigned char param);
216 
217  bool getAxes(int *ax) override;
218 
222  bool positionMove(int j, double ref) override;
223  bool positionMove(const double *refs) override;
224  bool relativeMove(int j, double delta) override;
225  bool relativeMove(const double *deltas) override;
226  bool checkMotionDone(int j, bool *flag) override;
227  bool checkMotionDone(bool *flag) override;
228  bool setRefSpeed(int j, double sp) override;
229  bool setRefSpeeds(const double *spds) override;
230  bool setRefAcceleration(int j, double acc) override;
231  bool setRefAccelerations(const double *accs) override;
232  bool getRefSpeed(int j, double *ref) override;
233  bool getRefSpeeds(double *spds) override;
234  bool getRefAcceleration(int j, double *acc) override;
235  bool getRefAccelerations(double *accs) override;
236  bool stop(int j) override;
237  bool stop() override;
238 
239  bool getRefTorques(double *t) override;
240  bool getRefTorque(int j, double *t) override;
241  bool setTorques(const double *t);
242  bool setTorque(int j, double t);
243  bool setTorquePid(int j, const Pid &pid);
244  bool getTorque(int j, double *t) override;
245  bool getTorques(double *t) override;
246  bool setTorquePids(const Pid *pids);
247  bool setTorqueErrorLimit(int j, double limit);
248  bool setTorqueErrorLimits(const double *limits);
249  bool getTorqueError(int j, double *err);
250  bool getTorqueErrors(double *errs);
251  bool getTorquePidOutput(int j, double *out);
252  bool getTorquePidOutputs(double *outs);
253  bool getTorquePid(int j, Pid *pid);
254  bool getTorquePids(Pid *pids);
255  bool getTorqueErrorLimit(int j, double *limit);
256  bool getTorqueErrorLimits(double *limits);
257  bool resetTorquePid(int j);
258  bool disableTorquePid(int j);
259  bool enableTorquePid(int j);
260  bool setTorqueOffset(int j, double v);
261 
262  bool getBemfParam(int j, double *bemf);
263  bool setBemfParam(int j, double bemf);
264 
265  bool resetEncoder(int j) override;
266  bool resetEncoders() override;
267 
268  bool setEncoder(int j, double val) override;
269  bool setEncoders(const double *vals) override;
273  bool getEncoder(int j, double *v) override;
274  bool getEncoders(double *encs) override;
275  bool getEncoderSpeed(int j, double *sp) override;
276  bool getEncoderSpeeds(double *spds) override;
277  bool getEncoderAcceleration(int j, double *spds) override;
278  bool getEncoderAccelerations(double *accs) override;
279 
280  using yarp::dev::ITorqueControl::setRefTorques;
281  bool setRefTorques(const double* t) override;
282 
283  bool setRefTorque(int j, double t) override;
284  bool getTorqueRange(int j, double* min, double* max) override;
285  bool getTorqueRanges(double* min, double* max) override;
286 
287  // multiple joint version
288  bool positionMove(const int n_joint, const int *joints, const double *refs) override;
289  bool relativeMove(const int n_joint, const int *joints, const double *deltas) override;
290  bool checkMotionDone(const int n_joint, const int *joints, bool *flag) override;
291  bool setRefSpeeds(const int n_joint, const int *joints, const double *spds) override;
292  bool setRefAccelerations(const int n_joint, const int *joints, const double *accs) override;
293  bool getRefSpeeds(const int n_joint, const int *joints, double *spds) override;
294  bool getRefAccelerations(const int n_joint, const int *joints, double *accs) override;
295  bool stop(const int n_joint, const int *joints) override;
296 
297 private:
298 
299  double *positions;
300  double *speeds;
301 
302  struct ftdi_device_list *devlist;
303  struct ftdi_device_list *curdev;
305  int *torques;
306 
307  int normalisePosition(double position);
308  int normaliseSpeed(double speed);
309 
315  bool initMotorIndex(yarp::os::Bottle *sensorIndex);
316 
317  int syncSendCommand(unsigned char id, unsigned char inst[], int size, unsigned char ret[], int &retSize);
318 
319  //bool lock; // lock to ensure sendcommand can be only accessed by one request at one time.
320 };
321 
322 //#undef __DEBUG__
323 
324 #endif
yarp::os::Bottle
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:73
FtdiDeviceSettings::vendor
int vendor
Definition: DynamixelAX12FtdiDriver.h:121
INSTRUCTION_ERROR
@ INSTRUCTION_ERROR
Definition: DynamixelAX12FtdiDriver.h:106
ErrorCode
ErrorCode
Definition: DynamixelAX12FtdiDriver.h:99
yarp::os::Searchable
A base class for nested structures that can be searched.
Definition: Searchable.h:69
t
float t
Definition: FfmpegWriter.cpp:74
yarp::dev::IEncoders
Control board, encoder interface.
Definition: IEncoders.h:121
yarp::dev::DeviceDriver
Interface implemented by all device drivers.
Definition: DeviceDriver.h:38
RANGE_ERROR
@ RANGE_ERROR
Definition: DynamixelAX12FtdiDriver.h:103
FtdiDeviceSettings::product
int product
Definition: DynamixelAX12FtdiDriver.h:122
ret
bool ret
Definition: ImplementAxisInfo.cpp:72
OK
@ OK
Definition: DynamixelAX12FtdiDriver.h:107
ControlBoardInterfaces.h
define control board standard interfaces
FtdiDeviceSettings::write_chunksize
unsigned int write_chunksize
Definition: DynamixelAX12FtdiDriver.h:133
OVERHEATING_ERROR
@ OVERHEATING_ERROR
Definition: DynamixelAX12FtdiDriver.h:102
FtdiDeviceSettings
FtdiDeviceSettings contains information to identify specific device Such a device can contain informa...
Definition: DynamixelAX12FtdiDriver.h:119
yarp::dev
An interface for the device drivers.
Definition: audioBufferSizeData.cpp:17
OVERLOAD_ERROR
@ OVERLOAD_ERROR
Definition: DynamixelAX12FtdiDriver.h:105
yarp::dev::ITorqueControl::setRefTorques
virtual bool setRefTorques(const double *t)=0
Set the reference value of the torque for all joints.
CHECKSUM_ERROR
@ CHECKSUM_ERROR
Definition: DynamixelAX12FtdiDriver.h:104
DynamixelAX12FtdiDriver
dynamixelAX12Ftdi: Documentation to be added
Definition: DynamixelAX12FtdiDriver.h:147
VOLTAGE_ERROR
@ VOLTAGE_ERROR
Definition: DynamixelAX12FtdiDriver.h:100
FtdiDeviceSettings::readtimeout
int readtimeout
Definition: DynamixelAX12FtdiDriver.h:127
FtdiDeviceSettings::read_chunksize
unsigned int read_chunksize
Definition: DynamixelAX12FtdiDriver.h:134
yarp::os
An interface to the operating system, including Port based communication.
Definition: AbstractCarrier.h:17
FtdiDeviceSettings::baudrate
int baudrate
Definition: DynamixelAX12FtdiDriver.h:126
yarp::dev::Pid
Contains the parameters for a PID.
Definition: ControlBoardPid.h:29
Time.h
yarp::dev::ITorqueControl
Interface for control boards implementing torque control.
Definition: ITorqueControl.h:39
yarp::dev::IPositionControl
Interface for a generic control board device implementing position control.
Definition: IPositionControl.h:257
FtdiDeviceSettings::flowctrol
int flowctrol
Definition: DynamixelAX12FtdiDriver.h:131
ANGLE_ERROR
@ ANGLE_ERROR
Definition: DynamixelAX12FtdiDriver.h:101
DeviceDriver.h