17#define BOOL_EXIT_FAILURE false
37 jointNumbers[0] = 0x65;
38 jointNumbers[1] = 0x74;
39 jointNumbers[2] = 0x84;
40 jointNumbers[3] = 0x66;
41 jointNumbers[4] = 0x75;
42 jointNumbers[5] = 0x85;
43 jointNumbers[6] = 0x67;
44 jointNumbers[7] = 0x76;
45 jointNumbers[8] = 0x86;
46 jointNumbers[9] = 0x6B;
47 jointNumbers[10] = 0x77;
48 jointNumbers[11] = 0x87;
49 jointNumbers[12] = 0x6A;
50 jointNumbers[13] = 0x68;
51 jointNumbers[14] = 0x69;
52 jointNumbers[15] = 0x6C;
74 ftdiSetting.baudrate = config.
check(
"baudrate",
Value(57600),
"Specifies the baudrate at which the communication port operates.").asInt32();
75 ftdiSetting.vendor = config.
check(
"ftdivendor",
Value(0x0403),
"USB device vendor. 0x0403 normally. Can be found by lsusb on linux").asInt32();
76 ftdiSetting.product = config.
check(
"ftdiproduct",
Value(0x6001),
"USB device product number. 0x6001 normally. Can be found by lsusb on linux").asInt32();
77 ftdiSetting.flowctrol = config.
check(
"flowctrl",
Value(
SIO_DISABLE_FLOW_CTRL),
"flow control to use. Should be SIO_DISABLE_FLOW_CTRL = 0x0, SIO_RTS_CTS_HS = 0x1 << 8, SIO_DTR_DSR_HS = 0x2 << 8, or SIO_XON_XOFF_HS = 0x4 << 8").asInt32();
86 char manufacturer[128], description[128];
112 for (curdev = devlist; curdev !=
NULL; i++) {
124 curdev = curdev->next;
127 if (curdev ==
NULL) {
148 if (ftdic.type ==
TYPE_R) {
209int DynamixelAX12FtdiDriver::syncSendCommand(
unsigned char id,
unsigned char inst[],
int size,
unsigned char ret[],
int &
retSize) {
225 unsigned char command[16];
226 unsigned char chksum = 0x00;
227 unsigned char header[4];
228 unsigned char body[256];
235 command[3] = size + 1;
238 for (i = 0; i < size; i++) {
239 command[4 + i] = inst[i];
244 command[size + 5 - 1] = ~(
chksum + command[2] + command[3]);
246 for (i = 0; i < size + 5; i++) {
275 if (header[0] != 0xFF || header[1] != 0xFF) {
278 }
else if (header[2] !=
id) {
293 for (i = 0; i <
retCode - 1; i++) {
296 if (body[
retCode - 1] != (
unsigned char) ~(
chksum + header[2] + header[3])) {
301 for (i = 0; i <
retCode - 1; i++) {
314 for (i = 0; i < 4; i++) {
317 for (i = 0; i <
retCode; i++) {
327ErrorCode DynamixelAX12FtdiDriver::checkAnswerPacket(
unsigned char*
packet,
const char*& message) {
329 message =
"Voltage out of operating voltage range!";
333 message =
"Goal position outside angle limits!";
337 message =
"Current temperature outside operating temperature!";
341 message =
"Instruction out of defined range!";
345 message =
"Checksum of instruction package incorrect!";
349 message =
"Specified torque can't control the applied load!";
353 message =
"Undefined instruction or missing Reg_Write instruction!";
369 const char* errorMessage;
404 const int instl = 5 + 1;
406 unsigned char inst[
instl] = {
INST_WRITE,
CT_GOAL_POSITION, (
unsigned char) (normalisePosition(ref)&0xFF), (
unsigned char) ((normalisePosition(ref) >> 8) & 0xFF), (
unsigned char) (normaliseSpeed(speed)&0xFF), (
unsigned char) ((normaliseSpeed(speed) >> 8)&0xFF)};
420 for (
int k = 0;
k < numOfAxes;
k++)
428 double v = positions[j];
432 return ReturnValue::return_code::return_value_error_method_failed;
438 for (
int k = 0;
k < numOfAxes;
k++) {
440 t = ReturnValue::return_code::return_value_error_method_failed;
458 for (
int k = 0;
k < numOfAxes;
k++)
461 t = ReturnValue::return_code::return_value_error_method_failed;
473 return ReturnValue::return_code::return_value_error_method_failed;
474 }
else if (sp > 114) {
477 return ReturnValue::return_code::return_value_error_method_failed;
486 for (
int k = 0;
k < numOfAxes;
k++)
513 for (
int k = 0;
k < numOfAxes;
k++) {
538 for (
int i = 0; i < numOfAxes; i++) {
540 return ReturnValue::return_code::return_value_error_method_failed;
546int DynamixelAX12FtdiDriver::normalisePosition(
double position) {
555 return (
int) (1024 *
position / 301);
558int DynamixelAX12FtdiDriver::normaliseSpeed(
double speed) {
564 }
else if (speed > 114) {
568 return (
int) (1024 * speed / 114 - 1);
572 for (
int k = 0;
k < numOfAxes;
k++) {
587 for (
int k = 0;
k < numOfAxes;
k++)
602 }
else if (t > 1023) {
609 const int instl = 3 + 1;
632 const char* message =
"";
641 return ReturnValue::return_code::return_value_error_method_failed;
649 for (
k = 0;
k < numOfAxes;
k++)
787 const char* message =
"";
790 *v = ((
double) pos)*300.0 / 1024.0;
794 return ReturnValue::return_code::return_value_error_method_failed;
799 return ReturnValue::return_code::return_value_error_method_failed;
807 for (
k = 0;
k < numOfAxes;
k++) {
824 const char* message =
"";
832 *sp = speed * 113 / 1024 + 1;
835 return ReturnValue::return_code::return_value_error_method_failed;
843 for (
k = 0;
k < numOfAxes;
k++)
866 tmp = (
unsigned char *)
realloc(jointNumbers, numOfAxes *
sizeof (
unsigned char));
874 tmpp = (
double *)
realloc(positions, numOfAxes *
sizeof (
double));
882 tmps = (
double *)
realloc(speeds, numOfAxes *
sizeof (
double));
890 tmpt = (
int *)
realloc(torques, numOfAxes *
sizeof (
int));
897 for (
int i = 0; i < numOfAxes; i++) {
927 for(
int j=0; j<joints.size(); j++)
981 ret &=
stop(joints[j]);
const yarp::os::LogComponent & DYNAMIXELAX12FTDIDRIVER()
bool NOT_YET_IMPLEMENTED(const char *txt)
#define BOOL_EXIT_FAILURE
#define AX12_BROADCAST_ID
defines for AX12 general ids
#define MOTION_COMPLETION_TOLERANCE
#define CT_PRESENT_POSITION
#define YARP_METHOD_NOT_YET_IMPLEMENTED()
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...
virtual std::string id() const
Return the id assigned to the PolyDriver.
Contains the parameters for a PID.
@ return_value_error_not_implemented_by_device
A simple collection of objects that can be described and transmitted in a portable way.
A mini-server for performing network communication in the background.
A base class for nested structures that can be searched.
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
A single value (typically within a Bottle).
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.