DynamixelAX12FtdiDriver.h

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
 * Copyright (C) 2010 Ze Ji
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms of the
 * BSD-3-Clause license. See the accompanying LICENSE file for details.
 */
 
 /*
 * Yarp Driver for Dynamixel AX-12, controlled using USB to Dynamixel Adapter
 * The default IDs for the motors are for Kaspar robot. As different robots would have different settings,
 * it is necessary to define the IDs, this can be done via a configuration file. In this example, check the
 * file sensorindex.cfg. The format is:
 * SENSORINDEX 16 101 116 132 102 117 133 103 118 134 107 119 135 106 104 105 108
 * This needs to be done by running configure(), before opening the device
 *
 * As it is using libftdi library, it is not possible to identify a device by specifying the serial port number, such as
 * /dev/ttyUSB0
 * 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.
 *
 * The motor does not support Torque control, but provide torque feedback. Therefore, several functions
 * have been implemented for such purpose, though not very rigorous.
 *
 * Author: Ze Ji - z.ji@herts.ac.uk (University of Hertfordshire, UK), 2010
 * as part of the RoboSkin project (EU FP7).
 * Part of the work is initially from Sven Magg, 2009
 */
 
#ifndef DynamixelAX12Driverh
#define DynamixelAX12Driverh
 
//#define __DEBUG__
 
#include <yarp/dev/DeviceDriver.h>
#include <yarp/dev/ControlBoardInterfaces.h>
 
#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <cstring>
#include <ftdi.h>
//#include <libftdi/ftdi.h>
#include <usb.h>
#include <iostream>
#include <yarp/os/Time.h>
 
#include <mutex>
 
#define MOTION_COMPLETION_TOLERANCE 3
 
#define INST_PING         0x01
#define INST_READ         0x02
#define INST_WRITE        0x03
#define INST_REGWRITE     0x04
#define INST_ACTION       0x05
#define INST_RESET        0x06
#define INST_SYNCWRITE    0x83
 
#define AX12_BROADCAST_ID 0xFE
 
#define CT_MODEL_NUMBER         00
#define CT_FIRMWARE_VERSION      2
#define CT_RETURN_DELAY_TIME     5
#define CT_CW_ANGLE_LIMIT       06
#define CT_CWW_ANGLE_LIMIT      08
#define CT_LIMIT_TEMPERATURE_H  11
#define CT_LIMIT_VOLTAGE_L      12
#define CT_LIMIT_VOLTAGE_H      13
#define CT_MAX_TORQUE           14
#define CT_STATUS_RETURN_LEVEL  16
 
#define CT_TORQUE_ENABLE        24
#define CT_CW_COMP_MARGIN       26
#define CT_CWW_COMP_MARGIN      27
#define CT_CW_COMP_SLOPE        28
#define CT_CWW_COMP_SLOPE       29
#define CT_GOAL_POSITION        30
#define CT_MOVING_SPEED         32
#define CT_TORQUE_LIMIT         34
#define CT_PRESENT_POSITION     0x24
#define CT_PRESENT_SPEED        0x26
#define CT_PRESENT_LOAD         0x28
#define CT_PRESENT_VOLTAGE      42
#define CT_PRESENT_TEMPERATURE  43
#define CT_REG_INSTRUCTION      44
#define CT_MOVING               46
#define CT_LOCK                 47
#define CT_PUNCH                48
 
enum ErrorCode {
    VOLTAGE_ERROR,
    ANGLE_ERROR,
    OVERHEATING_ERROR,
    RANGE_ERROR,
    CHECKSUM_ERROR,
    OVERLOAD_ERROR,
    INSTRUCTION_ERROR,
    OK
};
 
using namespace yarp::os;
using namespace yarp::dev;
 
class FtdiDeviceSettings
{
public:
    int vendor; //0x0403 normally. Can be found by lsusb on linux
    int product; //0x6001   Can be found by lsusb on linux
    char description[100]; // Contains the description
    char manufacturer[100]; // Contains manufacturer
    char serial[100]; // used mainly, as this seems unique
    int baudrate; // if ((retCode = ftdi_set_baudrate(&ftdic, config2.SerialParams.baudrate)) != 0)
    int readtimeout;
    // int bits;                    //    if (ftdi_set_line_property(&ftdic, BITS_8, STOP_BIT_1, NONE) == -1)
    // int parity;
    // int sbit;
    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
 
    unsigned int write_chunksize; // e.g. ftdi_write_data_set_chunksize(&ftdic, 3);
    unsigned int read_chunksize; // ftdi_read_data_set_chunksize(&ftdic, 256);
};
 
class DynamixelAX12FtdiDriver :
        public yarp::dev::DeviceDriver,
        public yarp::dev::IPositionControl,
        public yarp::dev::ITorqueControl,
        public yarp::dev::IEncoders
{
private:
    struct ftdi_context ftdic;
    bool deviceOpen;
 
    ErrorCode checkAnswerPacket(unsigned char* packet, const char*& message);
 
    std::mutex mutex;
 
    unsigned char *jointNumbers;
 
    int numOfAxes; // default = 16
 
public:
 
    DynamixelAX12FtdiDriver();
 
    virtual ~DynamixelAX12FtdiDriver();
 
    bool open(yarp::os::Searchable& config) override;
 
    bool close() override;
 
    bool configure(yarp::os::Searchable& config) override;
 
    virtual int sendCommand(unsigned char id, unsigned char inst[], int size, unsigned char ret[], int &retSize);
 
    virtual int readParameter(unsigned char id, unsigned char param);
 
    bool getAxes(int *ax) override;
 
    bool positionMove(int j, double ref) override;
    bool positionMove(const double *refs) override;
    bool relativeMove(int j, double delta) override;
    bool relativeMove(const double *deltas) override;
    bool checkMotionDone(int j, bool *flag) override;
    bool checkMotionDone(bool *flag) override;
    bool setRefSpeed(int j, double sp) override;
    bool setRefSpeeds(const double *spds) override;
    bool setRefAcceleration(int j, double acc) override;
    bool setRefAccelerations(const double *accs) override;
    bool getRefSpeed(int j, double *ref) override;
    bool getRefSpeeds(double *spds) override;
    bool getRefAcceleration(int j, double *acc) override;
    bool getRefAccelerations(double *accs) override;
    bool stop(int j) override;
    bool stop() override;
 
    bool getRefTorques(double *t) override;
    bool getRefTorque(int j, double *t) override;
    bool setTorques(const double *t);
    bool setTorque(int j, double t);
    bool setTorquePid(int j, const Pid &pid);
    bool getTorque(int j, double *t) override;
    bool getTorques(double *t) override;
    bool setTorquePids(const Pid *pids);
    bool setTorqueErrorLimit(int j, double limit);
    bool setTorqueErrorLimits(const double *limits);
    bool getTorqueError(int j, double *err);
    bool getTorqueErrors(double *errs);
    bool getTorquePidOutput(int j, double *out);
    bool getTorquePidOutputs(double *outs);
    bool getTorquePid(int j, Pid *pid);
    bool getTorquePids(Pid *pids);
    bool getTorqueErrorLimit(int j, double *limit);
    bool getTorqueErrorLimits(double *limits);
    bool resetTorquePid(int j);
    bool disableTorquePid(int j);
    bool enableTorquePid(int j);
    bool setTorqueOffset(int j, double v);
 
    bool getBemfParam(int j, double *bemf);
    bool setBemfParam(int j, double bemf);
 
    bool resetEncoder(int j) override;
    bool resetEncoders() override;
 
    bool setEncoder(int j, double val) override;
    bool setEncoders(const double *vals) override;
    bool getEncoder(int j, double *v) override;
    bool getEncoders(double *encs) override;
    bool getEncoderSpeed(int j, double *sp) override;
    bool getEncoderSpeeds(double *spds) override;
    bool getEncoderAcceleration(int j, double *spds) override;
    bool getEncoderAccelerations(double *accs) override;
 
    using yarp::dev::ITorqueControl::setRefTorques;
    bool setRefTorques(const double* t) override;
 
    bool setRefTorque(int j, double t) override;
    bool getTorqueRange(int j, double* min, double* max) override;
    bool getTorqueRanges(double* min, double* max) override;
 
    // multiple joint version
    bool positionMove(const int n_joint, const int *joints, const double *refs) override;
    bool relativeMove(const int n_joint, const int *joints, const double *deltas) override;
    bool checkMotionDone(const int n_joint, const int *joints, bool *flag) override;
    bool setRefSpeeds(const int n_joint, const int *joints, const double *spds) override;
    bool setRefAccelerations(const int n_joint, const int *joints, const double *accs) override;
    bool getRefSpeeds(const int n_joint, const int *joints, double *spds) override;
    bool getRefAccelerations(const int n_joint, const int *joints, double *accs) override;
    bool stop(const int n_joint, const int *joints) override;
 
private:
 
    double *positions;
    double *speeds;
 
    struct ftdi_device_list *devlist;
    struct ftdi_device_list *curdev;
    int *torques;
 
    int normalisePosition(double position);
    int normaliseSpeed(double speed);
 
    bool initMotorIndex(yarp::os::Bottle *sensorIndex);
 
    int syncSendCommand(unsigned char id, unsigned char inst[], int size, unsigned char ret[], int &retSize);
 
    //bool lock;    // lock to ensure sendcommand can be only accessed by one request at one time.
};
 
//#undef __DEBUG__
 
#endif