YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
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
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);
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
217 yarp::dev::ReturnValue getAxes(size_t& ax) override;
218
219 yarp::dev::ReturnValue positionMove(int j, double ref) override;
220 yarp::dev::ReturnValue positionMove(const double *refs) override;
221 yarp::dev::ReturnValue relativeMove(int j, double delta) override;
222 yarp::dev::ReturnValue relativeMove(const double *deltas) override;
223 yarp::dev::ReturnValue checkMotionDone(bool& flag) override;
224 yarp::dev::ReturnValue checkMotionDone(int j, bool& flag) override;
225 yarp::dev::ReturnValue setTrajSpeed(int j, double sp) override;
226 yarp::dev::ReturnValue setTrajSpeeds(const double *spds) override;
227 yarp::dev::ReturnValue setTrajAcceleration(int j, double acc) override;
228 yarp::dev::ReturnValue setTrajAccelerations(const double *accs) override;
229 yarp::dev::ReturnValue getTrajSpeed(int j, double *ref) override;
230 yarp::dev::ReturnValue getTrajSpeeds(double *spds) override;
231 yarp::dev::ReturnValue getTrajAcceleration(int j, double *acc) override;
232 yarp::dev::ReturnValue getTrajAccelerations(double *accs) override;
233 yarp::dev::ReturnValue stop(int j) override;
234 yarp::dev::ReturnValue stop() override;
235
236 yarp::dev::ReturnValue getRefTorques(double *t) override;
237 yarp::dev::ReturnValue getRefTorque(int j, double *t) override;
238 yarp::dev::ReturnValue setTorques(const double *t);
239 yarp::dev::ReturnValue setTorque(int j, double t);
240 yarp::dev::ReturnValue setTorquePid(int j, const Pid &pid);
241 yarp::dev::ReturnValue getTorque(int j, double *t) override;
242 yarp::dev::ReturnValue getTorques(double *t) override;
244 yarp::dev::ReturnValue setTorqueErrorLimit(int j, double limit);
245 yarp::dev::ReturnValue setTorqueErrorLimits(const double *limits);
246 yarp::dev::ReturnValue getTorqueError(int j, double *err);
248 yarp::dev::ReturnValue getTorquePidOutput(int j, double *out);
252 yarp::dev::ReturnValue getTorqueErrorLimit(int j, double *limit);
258
259 yarp::dev::ReturnValue getBemfParam(int j, double *bemf);
260 yarp::dev::ReturnValue setBemfParam(int j, double bemf);
261
262 yarp::dev::ReturnValue resetEncoder(int j) override;
264
265 yarp::dev::ReturnValue setEncoder(int j, double val) override;
266 yarp::dev::ReturnValue setEncoders(const double *vals) override;
270 yarp::dev::ReturnValue getEncoder(int j, double *v) override;
271 yarp::dev::ReturnValue getEncoders(double *encs) override;
272 yarp::dev::ReturnValue getEncoderSpeed(int j, double *sp) override;
273 yarp::dev::ReturnValue getEncoderSpeeds(double *spds) override;
274 yarp::dev::ReturnValue getEncoderAcceleration(int j, double *spds) override;
275 yarp::dev::ReturnValue getEncoderAccelerations(double *accs) override;
276
278 yarp::dev::ReturnValue setRefTorques(const double* t) override;
279
280 yarp::dev::ReturnValue setRefTorque(int j, double t) override;
281 yarp::dev::ReturnValue getTorqueRange(int j, double* min, double* max) override;
282 yarp::dev::ReturnValue getTorqueRanges(double* min, double* max) override;
283
284 // multiple joint version
285 yarp::dev::ReturnValue positionMove(const int n_joint, const int *joints, const double *refs) override;
286 yarp::dev::ReturnValue relativeMove(const int n_joint, const int *joints, const double *deltas) override;
287 yarp::dev::ReturnValue checkMotionDone(const std::vector<int>& joints, bool& flag) override;
288 yarp::dev::ReturnValue setTrajSpeeds(const int n_joint, const int *joints, const double *spds) override;
289 yarp::dev::ReturnValue setTrajAccelerations(const int n_joint, const int *joints, const double *accs) override;
290 yarp::dev::ReturnValue getTrajSpeeds(const int n_joint, const int *joints, double *spds) override;
291 yarp::dev::ReturnValue getTrajAccelerations(const int n_joint, const int *joints, double *accs) override;
292 yarp::dev::ReturnValue stop(const int n_joint, const int *joints) override;
293
294 yarp::dev::ReturnValue getTargetPosition(const int joint, double *ref) override;
295 yarp::dev::ReturnValue getTargetPositions(double *refs) override;
296 yarp::dev::ReturnValue getTargetPositions(const int n_joint, const int *joints, double *refs) override;
297 yarp::dev::ReturnValue setRefTorques(const int n_joint, const int *joints, const double *t) override;
300
301private:
302
303 double *positions;
304 double *speeds;
305
306 struct ftdi_device_list *devlist;
307 struct ftdi_device_list *curdev;
309 int *torques;
310
311 int normalisePosition(double position);
312 int normaliseSpeed(double speed);
313
319 bool initMotorIndex(yarp::os::Bottle *sensorIndex);
320
321 int syncSendCommand(unsigned char id, unsigned char inst[], int size, unsigned char ret[], int &retSize);
322
323 //bool lock; // lock to ensure sendcommand can be only accessed by one request at one time.
324};
325
326//#undef __DEBUG__
327
328#endif
define control board standard interfaces
@ OVERHEATING_ERROR
dynamixelAX12Ftdi: Documentation to be added
yarp::dev::ReturnValue getEncoderSpeed(int j, double *sp) override
Read the istantaneous speed of an axis.
yarp::dev::ReturnValue resetEncoders() override
Reset encoders.
yarp::dev::ReturnValue setTorqueErrorLimits(const double *limits)
yarp::dev::ReturnValue getTargetPositions(double *refs) override
Get the last position reference for all axes.
yarp::dev::ReturnValue getEncoderSpeeds(double *spds) override
Read the instantaneous speed of all axes.
yarp::dev::ReturnValue getTrajAccelerations(double *accs) override
not implemented
yarp::dev::ReturnValue relativeMove(int j, double delta) override
Set relative position.
yarp::dev::ReturnValue setTrajSpeed(int j, double sp) override
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
yarp::dev::ReturnValue setEncoder(int j, double val) override
Set the value of the encoder for a given joint.
yarp::dev::ReturnValue checkMotionDone(bool &flag) override
Check if the current trajectory is terminated.
yarp::dev::ReturnValue getTorquePid(int j, Pid *pid)
bool close() override
Close device Closes the device and shuts down connection.
yarp::dev::ReturnValue setRefTorques(const double *t) override
Set the reference value of the torque for all joints.
yarp::dev::ReturnValue setTrajSpeeds(const double *spds) override
Set reference speed on all joints.
bool configure(yarp::os::Searchable &config)
Configure device online Configures parts of the device that can be configures online.
yarp::dev::ReturnValue resetEncoder(int j) override
Reset encoder, single joint.
yarp::dev::ReturnValue setTrajAcceleration(int j, double acc) override
not implemented
yarp::dev::ReturnValue getTorquePidOutput(int j, double *out)
yarp::dev::ReturnValue getEncoderAccelerations(double *accs) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getTorqueRange(int j, double *min, double *max) override
Get the full scale of the torque sensor of a given joint.
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.
yarp::dev::ReturnValue getTrajSpeeds(double *spds) override
Get reference speed of all joints.
yarp::dev::ReturnValue positionMove(int j, double ref) override
Set new reference point for a single axis.
yarp::dev::ReturnValue getTorquePids(Pid *pids)
virtual int readParameter(unsigned char id, unsigned char param)
Read parameter from motor Requests the value of a parameter from motor.
yarp::dev::ReturnValue enableTorquePid(int j)
yarp::dev::ReturnValue getMotorTorqueParams(int j, yarp::dev::MotorTorqueParameters *params) override
Get a subset of motor parameters (bemf, ktau etc) useful for torque control.
yarp::dev::ReturnValue getEncoders(double *encs) override
Read the position of all axes.
yarp::dev::ReturnValue setTorquePid(int j, const Pid &pid)
yarp::dev::ReturnValue setEncoders(const double *vals) override
Set the value of all encoders.
yarp::dev::ReturnValue setTorqueOffset(int j, double v)
yarp::dev::ReturnValue getTorquePidOutputs(double *outs)
yarp::dev::ReturnValue resetTorquePid(int j)
yarp::dev::ReturnValue getTrajSpeed(int j, double *ref) override
Get reference speed for a joint.
yarp::dev::ReturnValue getTargetPosition(const int joint, double *ref) override
Get the last position reference for the specified axis.
bool open(yarp::os::Searchable &config) override
Open device Opens and configures the device.
yarp::dev::ReturnValue setTorquePids(const Pid *pids)
yarp::dev::ReturnValue setTorque(int j, double t)
yarp::dev::ReturnValue getTorqueErrorLimit(int j, double *limit)
yarp::dev::ReturnValue getBemfParam(int j, double *bemf)
yarp::dev::ReturnValue 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).
DynamixelAX12FtdiDriver()
Constructor Initializes handle but doesn't open channel yet.
yarp::dev::ReturnValue setBemfParam(int j, double bemf)
virtual ~DynamixelAX12FtdiDriver()
Destructor Closes connection and destroys device object.
yarp::dev::ReturnValue setTorques(const double *t)
yarp::dev::ReturnValue getTorqueErrors(double *errs)
yarp::dev::ReturnValue setTrajAccelerations(const double *accs) override
not implemented
yarp::dev::ReturnValue getEncoderAcceleration(int j, double *spds) override
Read the instantaneous acceleration of an axis.
yarp::dev::ReturnValue setMotorTorqueParams(int j, const yarp::dev::MotorTorqueParameters params) override
Set a subset of motor parameters (bemf, ktau etc) useful for torque control.
yarp::dev::ReturnValue getRefTorques(double *t) override
Get the reference value of the torque for all joints.
yarp::dev::ReturnValue setRefTorque(int j, double t) override
Set the reference value of the torque for a given joint.
yarp::dev::ReturnValue setTorqueErrorLimit(int j, double limit)
yarp::dev::ReturnValue getTrajAcceleration(int j, double *acc) override
not implemented
yarp::dev::ReturnValue getTorqueError(int j, double *err)
yarp::dev::ReturnValue getTorqueErrorLimits(double *limits)
yarp::dev::ReturnValue getTorques(double *t) override
Get the value of the torque for all joints (this is the feedback if you have torque sensors).
yarp::dev::ReturnValue stop() override
Stop motion, multiple joints.
yarp::dev::ReturnValue getTorqueRanges(double *min, double *max) override
Get the full scale of the torque sensors of all joints.
yarp::dev::ReturnValue disableTorquePid(int j)
yarp::dev::ReturnValue getRefTorque(int j, double *t) override
Get the reference value of the torque for a given joint.
yarp::dev::ReturnValue getAxes(size_t &ax) override
Interfaces methods.
yarp::dev::ReturnValue getEncoder(int j, double *v) override
Get the angle of servo.
FtdiDeviceSettings contains information to identify specific device Such a device can contain informa...
Interface implemented by all device drivers.
Control board, encoder interface.
Definition IEncoders.h:117
Interface for a generic control board device implementing position control.
Interface for control boards implementing torque control.
virtual yarp::dev::ReturnValue 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:65
A mini-server for performing network communication in the background.
A base class for nested structures that can be searched.
Definition Searchable.h:31
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.
Definition OSTestMsg.cpp:13