YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
DynamixelAX12FtdiDriver.cpp
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
8
9#include <yarp/os/Value.h>
11
12#include <cstdio>
13
14using namespace yarp::os;
15using namespace yarp::dev;
16
17#define BOOL_EXIT_FAILURE false
18
19YARP_LOG_COMPONENT(DYNAMIXELAX12FTDIDRIVER, "yarp.devices.DynamixelAX12FtdiDriver")
20
21
23 yCError(DYNAMIXELAX12FTDIDRIVER, "%s not yet implemented for DynamixelAX12FtdiDriver", txt);
24 return false;
25}
26
28 deviceOpen(false),
29 jointNumbers((unsigned char *) malloc(16 * sizeof (unsigned char))),
30 numOfAxes(16),
31 positions((double *) malloc(numOfAxes * sizeof (double))),
32 speeds((double *) malloc(numOfAxes * sizeof (double))),
33 devlist(NULL),
34 torques((int *) malloc(numOfAxes * sizeof (int)))
35{
36 // initialise
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;
53
54 ftdi_init(&ftdic);
55}
56
58 ftdi_list_free(&devlist);
59 ftdi_deinit(&ftdic);
60 free(jointNumbers);
61 free(positions);
62 free(speeds);
63 free(torques);
64}
65
67
68 mutex.lock();
69
71 strcpy(ftdiSetting.description, config.check("FTDI_DESCRIPTION", Value("FT232R USB UART"), "Ftdi device description").asString().c_str());
72 strcpy(ftdiSetting.serial, config.check("FTDI_SERIAL", Value("A7003MhG"), "Ftdi device serial").asString().c_str());
73 strcpy(ftdiSetting.manufacturer, config.check("FTDI_MANUFACTURER", Value("FTDI"), "Ftdi device manufacturer").asString().c_str());
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();
78 ftdiSetting.write_chunksize = 3;
79 ftdiSetting.read_chunksize = 256;
80
81 yCTrace(DYNAMIXELAX12FTDIDRIVER, "DynamixelAx12Driver::initialize");
82 yCInfo(DYNAMIXELAX12FTDIDRIVER, "Opening DynamixelAx12 Device");
83
84 int retCode;
85
86 char manufacturer[128], description[128];
87 char serial[128];
88 //int n = 0; // number of devices found
89
90 retCode = ftdi_usb_find_all(&ftdic, &devlist, ftdiSetting.vendor, ftdiSetting.product);
91 switch (retCode) {
92 case -1:
93 yCError(DYNAMIXELAX12FTDIDRIVER, "usb_find_busses() failed");
94 return BOOL_EXIT_FAILURE;
95 case -2:
96 yCError(DYNAMIXELAX12FTDIDRIVER, "usb_find_devices() failed");
97 return BOOL_EXIT_FAILURE;
98 case -3:
99 yCError(DYNAMIXELAX12FTDIDRIVER, "out of memory");
100 return BOOL_EXIT_FAILURE;
101 case 0:
102 yCError(DYNAMIXELAX12FTDIDRIVER, "No device is found. Check connection.");
103 return BOOL_EXIT_FAILURE;
104 default:
105 yCInfo(DYNAMIXELAX12FTDIDRIVER, "%d devices have been found.", retCode);
106 break;
107 }
108
109 int ret = 0;
110 int i = 0;
111
112 for (curdev = devlist; curdev != NULL; i++) {
113 yCInfo(DYNAMIXELAX12FTDIDRIVER, "Checking device: %d", i);
114
115 if ((retCode = ftdi_usb_get_strings(&ftdic, curdev->dev, manufacturer, 128, description, 128, serial, 128)) < 0) {
116 yCError(DYNAMIXELAX12FTDIDRIVER, "ftdi_usb_get_strings failed: %d (%s)", ret, ftdi_get_error_string(&ftdic));
117 }
118 yCInfo(DYNAMIXELAX12FTDIDRIVER, "Manufacturer: %s, Description: %s, Serial %s", manufacturer, description, serial);
119
120 // check if the current device is the right one
121 if (strcmp(serial, ftdiSetting.serial) == 0) {
122 break;
123 }
124 curdev = curdev->next;
125 }
126
127 if (curdev == NULL) {
128 yCError(DYNAMIXELAX12FTDIDRIVER, "No Dynamixel device is found. Check connection.");
129 return BOOL_EXIT_FAILURE;
130 }
131
132 // open and reset everything in case we messed everything up with a control-c..
133 if ((retCode = ftdi_usb_open_dev(&ftdic, curdev->dev)) < 0) {
134 yCError(DYNAMIXELAX12FTDIDRIVER, "unable to open ftdi device: %d (%s)", retCode, ftdi_get_error_string(&ftdic));
135 return BOOL_EXIT_FAILURE;
136 }
137 deviceOpen = true; // Only set to be able to do close()
138
139 this->close();
140
141 // Now open again
142 if ((retCode = ftdi_usb_open_dev(&ftdic, curdev->dev)) < 0) {
143 yCError(DYNAMIXELAX12FTDIDRIVER, "unable to open ftdi device: %d (%s)", retCode, ftdi_get_error_string(&ftdic));
144 return BOOL_EXIT_FAILURE;
145 }
146
147 // To test connection, read out FTDIChip-ID of R type chips
148 if (ftdic.type == TYPE_R) {
149 unsigned int chipid;
150 if (ftdi_read_chipid(&ftdic, &chipid) == 0) {
151 yCInfo(DYNAMIXELAX12FTDIDRIVER, "FTDI Device open. FTDI chipid: %X", chipid);
152 } else {
153 yCError(DYNAMIXELAX12FTDIDRIVER, "Unable to access FTDI device!");
154 return BOOL_EXIT_FAILURE;
155 }
156 }
157 deviceOpen = true;
158
159 // We have an open device and it is accessible, so set parameters
160
161 if ((retCode = ftdi_set_baudrate(&ftdic, ftdiSetting.baudrate)) != 0) {
162 yCError(DYNAMIXELAX12FTDIDRIVER, "Error setting baudrate, ret=%d", retCode);
163 }
164 if (ftdi_set_line_property(&ftdic, BITS_8, STOP_BIT_1, NONE) == -1) {
165 yCError(DYNAMIXELAX12FTDIDRIVER, "Error setting connection properties");
166 }
167 if (ftdi_setflowctrl(&ftdic, SIO_DISABLE_FLOW_CTRL) == -1) {
168 yCError(DYNAMIXELAX12FTDIDRIVER, "Error setting flow control");
169 }
170
171 // Set chunk sizes for in and out
172 ftdi_write_data_set_chunksize(&ftdic, ftdiSetting.write_chunksize);
173 ftdi_read_data_set_chunksize(&ftdic, ftdiSetting.read_chunksize);
174
175 // Everything ready to rumble
176
177 mutex.unlock();
178
179 return true;
180}
181
183
184 // First check if we have an open device
185 if (deviceOpen) {
186 // Yes, we have, so purge the buffers, reset the device and then close it
188 //ftdi_usb_reset(ftdic.usb_dev);
189 ftdi_usb_reset(&ftdic);
190 ftdi_usb_close(&ftdic);
191 } else {
192 // We don't seem to have a device open at this time
193 yCError(DYNAMIXELAX12FTDIDRIVER, "close():No device open to be be closed!");
194 return false;
195 }
196 return true;
197}
198
203 yarp::os::Value indexValue = config.find("SENSORINDEX");
205 this->initMotorIndex(indexBottle);
206 return true;
207}
208
209int DynamixelAX12FtdiDriver::syncSendCommand(unsigned char id, unsigned char inst[], int size, unsigned char ret[], int &retSize) {
210 int r = 0;
211 mutex.lock();
212 if (!ftdi_usb_purge_buffers(&ftdic)) {
213 r = sendCommand(id, inst, size, ret, retSize);
214 }
215
216 mutex.unlock();
217 return r;
218}
219
220int DynamixelAX12FtdiDriver::sendCommand(unsigned char id, unsigned char inst[], int size, unsigned char ret[], int &retSize) {
221
222 int i;
223 int retCode = 0;
224 bool badAnswer = false;
225 unsigned char command[16];
226 unsigned char chksum = 0x00;
227 unsigned char header[4];
228 unsigned char body[256];
229
230 // Build packet header
231
232 command[0] = 0xFF; // Header bytes
233 command[1] = 0xFF;
234 command[2] = id; // Id of device
235 command[3] = size + 1; // Size of instruction plus checksum byte
236
237 // Add instruction
238 for (i = 0; i < size; i++) {
239 command[4 + i] = inst[i];
240 chksum += inst[i];
241 }
242
243 // Calculate checksum
244 command[size + 5 - 1] = ~(chksum + command[2] + command[3]);
245 yCDebug(DYNAMIXELAX12FTDIDRIVER, "Packet sent: ");
246 for (i = 0; i < size + 5; i++) {
247 yCDebug(DYNAMIXELAX12FTDIDRIVER, " %X", command[i]);
248 }
249 // Write data to device
250 retCode = ftdi_write_data(&ftdic, command, size + 5);
251
252 // Now receive return packet
253 if (id != AX12_BROADCAST_ID) {
254 do {
255 //retCode stores the number of bytes read, which may not correspond to the number of bytes
256 //one wants to read (the third parameter). This is a timing issue, so to learn how to fix
257 //it, I should send a command and then continually prints the bytes that I get back. At
258 //some point, I will get back the correct number of bytes. After doing this enough times,
259 //I should work out the timing issue
260 retCode = ftdi_read_data(&ftdic, header, 4);
261 } // retCode: <0,: error code from usb_bulk_read()
262 // 0: no data was available
263 // >0: number of bytes read
265 while (retCode == 0); // not needed. as it will hang if no device is connected
266
267 if (retCode < 0) {
268 yCError(DYNAMIXELAX12FTDIDRIVER, "Error while reading header of status packet! From usb_bulk_read() (%d)", retCode);
269 return 0;
270 } else if (retCode < 4) {
271 yCError(DYNAMIXELAX12FTDIDRIVER, "Error while reading header of status packet! (%d)", retCode);
272 return 0;
273 } else {
274 retCode = 0;
275 if (header[0] != 0xFF || header[1] != 0xFF) {
276 badAnswer = true;
277 yCInfo(DYNAMIXELAX12FTDIDRIVER, "Received data without header bytes: ");
278 } else if (header[2] != id) {
279 badAnswer = true;
280 yCInfo(DYNAMIXELAX12FTDIDRIVER, "Received status from wrong device (expected %d): ", id);
281 } else {
282 // Now that we know a well formed packet is arriving, read remaining bytes
283 do {
284 retCode = ftdi_read_data(&ftdic, body, header[3]);
285 } while (retCode == 0);
286
287 if (retCode != header[3]) {
288 badAnswer = true;
289 yCInfo(DYNAMIXELAX12FTDIDRIVER, "Received data with wrong length: ");
290 } else {
291 //check checksum
292 chksum = 0;
293 for (i = 0; i < retCode - 1; i++) {
294 chksum += body[i];
295 }
296 if (body[retCode - 1] != (unsigned char) ~(chksum + header[2] + header[3])) {
297 badAnswer = true;
298 yCInfo(DYNAMIXELAX12FTDIDRIVER, "Received data with wrong checksum (%X != %X): ", body[retCode - 1], (unsigned char) ~(chksum + header[2] + header[3]));
299 } else {
300 // Packet ok, so return the instruction part without checksum
301 for (i = 0; i < retCode - 1; i++) {
302 ret[i] = body[i];
303 }
304 // retSize = retCode; // Sven's original code.
305 retSize = retCode - 1; // retSize should be retCode - 1, as checksum is not included
306 return 1;
307 }
308 }
309 }
310 }
311
312 // Print packet if something was wrong
313 if (badAnswer) {
314 for (i = 0; i < 4; i++) {
315 yCInfo(DYNAMIXELAX12FTDIDRIVER, "%X ", header[i]);
316 }
317 for (i = 0; i < retCode; i++) {
318 yCInfo(DYNAMIXELAX12FTDIDRIVER, "%X ", body[i]);
319 }
320 return 0;
321 }
322 }
323
324 return 1;
325}
326
327ErrorCode DynamixelAX12FtdiDriver::checkAnswerPacket(unsigned char* packet, const char*& message) {
328 if (packet[0] & 1) {
329 message = "Voltage out of operating voltage range!";
330 return VOLTAGE_ERROR;
331 }
332 if (packet[0] & 2) {
333 message = "Goal position outside angle limits!";
334 return ANGLE_ERROR;
335 }
336 if (packet[0] & 4) {
337 message = "Current temperature outside operating temperature!";
338 return OVERHEATING_ERROR;
339 }
340 if (packet[0] & 8) {
341 message = "Instruction out of defined range!";
342 return RANGE_ERROR;
343 }
344 if (packet[0] & 16) {
345 message = "Checksum of instruction package incorrect!";
346 return CHECKSUM_ERROR;
347 }
348 if (packet[0] & 32) {
349 message = "Specified torque can't control the applied load!";
350 return OVERLOAD_ERROR;
351 }
352 if (packet[0] & 64) {
353 message = "Undefined instruction or missing Reg_Write instruction!";
354 return INSTRUCTION_ERROR;
355 }
356 message = "";
357 return OK;
358}
359
362int DynamixelAX12FtdiDriver::readParameter(unsigned char id, unsigned char param) {
363 // Read instruction has 2 parameters: The starting address of the parameter and the number of bytes to read
364 // Both are encoded in param (> 100 = 2 bytes)
365 unsigned char packet[] = {INST_READ, 0, 1};
366
367 unsigned char answerPacket[256];
368 int answerSize;
369 const char* errorMessage;
371
372 if (param > 99) {
373 // We have to read 2 bytes
374 packet[1] = param - 100;
375 packet[2] = 2;
376 } else {
377 // We have to read 1 bytes
378 packet[1] = param;
379 }
380
381 // Send request
382 //sendCommand(id, packet, 3, answerPacket, answerSize);
383 syncSendCommand(id, packet, 3, answerPacket, answerSize);
384 // Check for submitted error code
385 if ((errCode = checkAnswerPacket(answerPacket, errorMessage)) != OK) {
386 // We received an error code
387 return -1;
388 } else {
389 // No error
390 return 1;
391 }
392}
393
395 ax = numOfAxes;
396 return ReturnValue_ok;
397}
398
400 double speed;
401 int blankReturnSize = -1;
402 unsigned char blankReturn[] = {0, 0, 0};
403 getTrajSpeed(j, &speed);
404 const int instl = 5 + 1;
405
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)};
407
408 positions[j] = ref;
409
410 if (syncSendCommand(jointNumbers[j], inst, instl, blankReturn, blankReturnSize))
411 {return ReturnValue_ok;}
413}
414
420 for (int k = 0; k < numOfAxes; k++)
421 {
422 t &= this->positionMove(k, refs[k]);
423 }
424 return t;
425}
426
428 double v = positions[j];
429 if (getEncoder(j, &v)) {
430 return this->positionMove(j, v + delta);
431 } else {
432 return ReturnValue::return_code::return_value_error_method_failed;
433 }
434}
435
438 for (int k = 0; k < numOfAxes; k++) {
439 if (!this->positionMove(k, positions[k] + deltas[k])) {
440 t = ReturnValue::return_code::return_value_error_method_failed;
441 }
442 }
443 return t;
444}
445
447 double v = 0;
449
450 t = getEncoder(j, &v);
451 flag = (std::fabs(v - positions[j]) < MOTION_COMPLETION_TOLERANCE);
452 return t;
453}
454
457 bool tmp_done(false), all_done(true);
458 for (int k = 0; k < numOfAxes; k++)
459 {
460 if (!this->checkMotionDone(k, tmp_done)) {
461 t = ReturnValue::return_code::return_value_error_method_failed;
462 }
464 }
465 flag = all_done;
466 return t;
467}
468
470 if (sp < 1) {
471 yCInfo(DYNAMIXELAX12FTDIDRIVER, "Invalid speed value, should be from 1 to 114");
472 speeds[j] = 1;
473 return ReturnValue::return_code::return_value_error_method_failed;
474 } else if (sp > 114) {
475 yCInfo(DYNAMIXELAX12FTDIDRIVER, "Invalid speed value, should be from 1 to 114");
476 speeds[j] = 114;
477 return ReturnValue::return_code::return_value_error_method_failed;
478 } else {
479 speeds[j] = sp;
480 return ReturnValue_ok;
481 }
482}
483
486 for (int k = 0; k < numOfAxes; k++)
487 {
488 ret &= setTrajSpeed(k, spds[k]);
489 }
490 return ret;
491}
492
499
506
508 *ref = speeds[j];
509 return ReturnValue_ok;
510}
511
513 for (int k = 0; k < numOfAxes; k++) {
514 spds[k] = speeds[k];
515 }
516 return ReturnValue_ok;
517}
518
525
532
536
538 for (int i = 0; i < numOfAxes; i++) {
539 if (!stop(i)) {
540 return ReturnValue::return_code::return_value_error_method_failed;
541 }
542 }
543 return ReturnValue_ok;
544}
545
546int DynamixelAX12FtdiDriver::normalisePosition(double position) {
547 if (position < 0) {
548 position = 0;
549 yCInfo(DYNAMIXELAX12FTDIDRIVER, "Invalid position value, should be from 0 to 300");
550 } else if (position > 300) {
551 position = 300;
552 yCInfo(DYNAMIXELAX12FTDIDRIVER, "Invalid position value, should be from 0 to 300");
553 }
554
555 return (int) (1024 * position / 301);
556}
557
558int DynamixelAX12FtdiDriver::normaliseSpeed(double speed) {
559 // speed -> [1 114] RPM
560 // when speed is 0, it is maximum possible speed, no speed control. so it is not useful here
561 if (speed < 1) {
562 yCInfo(DYNAMIXELAX12FTDIDRIVER, "Invalid speed value, should be from 1 to 114");
563 speed = 1;
564 } else if (speed > 114) {
565 yCInfo(DYNAMIXELAX12FTDIDRIVER, "Invalid speed value, should be from 1 to 114");
566 speed = 114;
567 }
568 return (int) (1024 * speed / 114 - 1);
569}
570
572 for (int k = 0; k < numOfAxes; k++) {
573 t[k] = torques[k];
574 }
575 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is just the MAX tourque set for the ax12 servo.");
576 return ReturnValue_ok;
577}
578
580 *t = torques[j];
581 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is just the MAX tourque set for the ax12 servo.");
582 return ReturnValue_ok;
583}
584
587 for (int k = 0; k < numOfAxes; k++)
588 {
589 ret &= this->setTorque(k, t[k]);
590 }
591 return ret;
592}
593
599 if (t < 0) {
600 yCError(DYNAMIXELAX12FTDIDRIVER, "torque (%d) should in range [0 1023] or [0 0x3FF]. t is set to 0 here", (int)t);
601 t = 0;
602 } else if (t > 1023) {
603 yCError(DYNAMIXELAX12FTDIDRIVER, "torque (%d) should in range [0 1023] or [0 0x3FF]. t is set to 1023 here", (int)t);
604 t = 1023;
605 }
606
607 int blankReturnSize = -1;
608 unsigned char blankReturn[] = {0, 0, 0};
609 const int instl = 3 + 1;
610
611 unsigned char inst[instl] = {INST_WRITE, CT_MAX_TORQUE, (unsigned char) ((int) t & 0xFF), (unsigned char) (((int) t << 8) & 0x03)};
612 //return sendCommand(jointNumbers[j], inst, instl, blankReturn, blankReturnSize);
613 return syncSendCommand(jointNumbers[j], inst, instl, blankReturn, blankReturnSize)==true?ReturnValue_ok:ReturnValue::return_code::return_value_error_method_failed;
614}
615
617 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is only used to get torque feedback.");
619}
620
622 ReturnValue ret;
623
624 int blankReturnSize = -1;
625 unsigned char blankReturn[] = {0, 0, 0};
626
627 const int instl = 3;
628 unsigned char inst[instl] = {INST_READ, (unsigned char) CT_PRESENT_LOAD, (unsigned char) 2};
629 //ret = sendCommand(jointNumbers[j], inst, instl, blankReturn, blankReturnSize);
630 ret = syncSendCommand(jointNumbers[j], inst, instl, blankReturn, blankReturnSize)==true?ReturnValue_ok:ReturnValue::return_code::return_value_error_method_failed;
631
632 const char* message = "";
633 if (checkAnswerPacket(blankReturn, message) == OK) {
634 //int load = (blankReturn[2]&0b00000011)*256 + blankReturn[1];
635 int load = (blankReturn[2]&0X03)*256 + blankReturn[1];
636 //load *= (((blankReturn[2] >> 2)&0b000001) ? -1 : 1);
637 load *= (((blankReturn[2] >> 2)&0X01) ? -1 : 1);
638 *t = load;
639 } else {
640 yCError(DYNAMIXELAX12FTDIDRIVER, "%s", message);
641 return ReturnValue::return_code::return_value_error_method_failed;
642 }
643 return ret;
644}
645
647 int k = 0;
649 for (k = 0; k < numOfAxes; k++)
650 {
651 ret &= getTorque(k, &t[k]);
652 }
653 return ret;
654}
655
657 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is only used to get torque feedback.");
659}
660
662 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is only used to get torque feedback.");
664}
665
667 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is only used to get torque feedback.");
669}
670
672 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is only used to get torque feedback.");
674}
675
677 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is only used to get torque feedback.");
679}
680
682 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is only used to get torque feedback.");
684}
685
687 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is only used to get torque feedback.");
689}
690
692 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is only used to get torque feedback.");
694}
695
697 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is only used to get torque feedback.");
699}
700
702 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is only used to get torque feedback.");
704}
705
707 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is only used to get torque feedback.");
709}
710
712 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is only used to get torque feedback.");
714}
715
717 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is only used to get torque feedback.");
719}
720
722 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is only used to get torque feedback.");
724}
725
727 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is only used to get torque feedback.");
729}
730
732 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is only used to get torque feedback.");
734}
735
737 yCError(DYNAMIXELAX12FTDIDRIVER, "Note: AX12 does not support torque control mode. This is only used to get torque feedback.");
739}
740
744
748
752
756
760
764
765ReturnValue DynamixelAX12FtdiDriver::getTorqueRange(int j, double* min, double* max) {
767}
768
772
774 *v = -1; // invalid value for encoder of AX12 motor
775 int ret = 1;
776
777 int blankReturnSize = -1;
778 unsigned char blankReturn[] = {0, 0, 0};
779
780 const int instl = 3;
781 unsigned char inst[instl] = {INST_READ, (unsigned char) CT_PRESENT_POSITION, (unsigned char) 2};
782 //ret = sendCommand(jointNumbers[j], inst, instl, blankReturn, blankReturnSize);
783 ret = syncSendCommand(jointNumbers[j], inst, instl, blankReturn, blankReturnSize);
784
785 int pos = 0;
786 if (ret == 1) {
787 const char* message = "";
788 if (checkAnswerPacket(blankReturn, message) == OK) {
789 pos = ((int) blankReturn[2])*256 + blankReturn[1];
790 *v = ((double) pos)*300.0 / 1024.0;
791 return ReturnValue_ok;
792 } else {
793 yCError(DYNAMIXELAX12FTDIDRIVER, "%s", message);
794 return ReturnValue::return_code::return_value_error_method_failed;
795 }
796
797 } // pos = (blankReturn[1]&0b00000011)*0x100 + blankReturn[0];
798 else {
799 return ReturnValue::return_code::return_value_error_method_failed;
800 }
801}
802
804 int k = 0;
806
807 for (k = 0; k < numOfAxes; k++) {
808 tt &= getEncoder(k, &encs[k]);
809 }
810 return tt;
811}
812
814 ReturnValue ret;
815
816 int blankReturnSize = -1;
817 unsigned char blankReturn[] = {0, 0, 0};
818 const int instl = 3;
819 unsigned char inst[instl] = {INST_READ, (unsigned char) CT_PRESENT_SPEED, (unsigned char) 2};
820 //ret = sendCommand(jointNumbers[j], inst, instl, blankReturn, blankReturnSize);
821 ret = syncSendCommand(jointNumbers[j], inst, instl, blankReturn, blankReturnSize)==true?ReturnValue_ok:ReturnValue::return_code::return_value_error_method_failed;
822
823 double speed = 0;
824 const char* message = "";
825 if (checkAnswerPacket(blankReturn, message) == OK) {
826 //speed = (blankReturn[2]&0b00000011)*0xFF + blankReturn[1];
827 speed = (blankReturn[2]&0X03)*0xFF + blankReturn[1];
828
829 //speed *= (((blankReturn[1] >> 2)&0b000001) ? -1 : 1);
830 speed *= (((blankReturn[1] >> 2)&0X01) ? -1 : 1);
831
832 *sp = speed * 113 / 1024 + 1;
833 } else {
834 yCError(DYNAMIXELAX12FTDIDRIVER, "%s", message);
835 return ReturnValue::return_code::return_value_error_method_failed;
836 }
837 return ret;
838}
839
841 int k = 0;
843 for (k = 0; k < numOfAxes; k++)
844 {
845 ret &= getEncoderSpeed(k, &spds[k]);
846 }
847 return ret;
848}
849
853
857
858bool DynamixelAX12FtdiDriver::initMotorIndex(yarp::os::Bottle *sensorIndex) {
859 int s = sensorIndex->size() - 1;
860 if (s != sensorIndex->get(0).asInt32()) {
861 yCError(DYNAMIXELAX12FTDIDRIVER, "sensor number does not match the real number in configuration file");
862 return false;
863 }
864 numOfAxes = s;
865 unsigned char *tmp = NULL;
866 tmp = (unsigned char *) realloc(jointNumbers, numOfAxes * sizeof (unsigned char));
867 if (tmp != NULL) {
868 // jointNumbers = {0x65, 0x74, 0x84, 0x66, 0x75, 0x85, 0x67, 0x76, 0x86, 0x6B, 0x77, 0x87, 0x6A, 0x68, 0x69, 0x6C};
869 jointNumbers = tmp;
870 } else {
871 return false;
872 }
873 double *tmpp = NULL;
874 tmpp = (double *) realloc(positions, numOfAxes * sizeof (double));
875 if (tmpp != NULL) {
876 positions = tmpp;
877 } else {
878 return false;
879 }
880
881 double *tmps = NULL;
882 tmps = (double *) realloc(speeds, numOfAxes * sizeof (double));
883 if (tmps != NULL) {
884 speeds = tmps;
885 } else {
886 return false;
887 }
888
889 int *tmpt = NULL;
890 tmpt = (int *) realloc(torques, numOfAxes * sizeof (int));
891 if (tmpt != NULL) {
892 torques = tmpt;
893 } else {
894 return false;
895 }
896
897 for (int i = 0; i < numOfAxes; i++) {
898 jointNumbers[i] = (unsigned char) (sensorIndex->get(i + 1).asInt32());
899 }
900 return true;
901}
902
903ReturnValue DynamixelAX12FtdiDriver::positionMove(const int n_joint, const int *joints, const double *refs)
904{
906 for(int j=0; j<n_joint; j++)
907 {
908 ret &= positionMove(joints[j], refs[j]);
909 }
910 return ret;
911}
912
913ReturnValue DynamixelAX12FtdiDriver::relativeMove(const int n_joint, const int *joints, const double *deltas)
914{
916 for(int j=0; j<n_joint; j++)
917 {
918 ret &= relativeMove(joints[j], deltas[j]);
919 }
920 return ret;
921}
922
923ReturnValue DynamixelAX12FtdiDriver::checkMotionDone(const std::vector<int>& joints, bool& flag)
924{
926 bool tmp_joint(false), tmp_device(true);
927 for(int j=0; j<joints.size(); j++)
928 {
929 ret &= checkMotionDone(joints[j], tmp_joint);
931 }
932 flag = tmp_device;
933 return ret;
934}
935
936ReturnValue DynamixelAX12FtdiDriver::setTrajSpeeds(const int n_joint, const int *joints, const double *spds)
937{
939 for(int j=0; j<n_joint; j++)
940 {
941 ret &= setTrajSpeed(joints[j], spds[j]);
942 }
943 return ret;
944}
945
946ReturnValue DynamixelAX12FtdiDriver::setTrajAccelerations(const int n_joint, const int *joints, const double *accs)
947{
949 for(int j=0; j<n_joint; j++)
950 {
951 ret &= setTrajAcceleration(joints[j], accs[j]);
952 }
953 return ret;
954}
955
956ReturnValue DynamixelAX12FtdiDriver::getTrajSpeeds(const int n_joint, const int *joints, double *spds)
957{
959 for(int j=0; j<n_joint; j++)
960 {
961 ret &= getTrajSpeed(joints[j], &spds[j]);
962 }
963 return ret;
964}
965
966ReturnValue DynamixelAX12FtdiDriver::getTrajAccelerations(const int n_joint, const int *joints, double *accs)
967{
969 for(int j=0; j<n_joint; j++)
970 {
971 ret &= getTrajSpeed(joints[j], &accs[j]);
972 }
973 return ret;
974}
975
977{
979 for(int j=0; j<n_joint; j++)
980 {
981 ret &= stop(joints[j]);
982 }
983 return ret;
984}
985
990
995
996ReturnValue DynamixelAX12FtdiDriver::getTargetPositions(const int n_joint, const int *joints, double *refs)
997{
999}
1000
1001ReturnValue DynamixelAX12FtdiDriver::setRefTorques(const int n_joint, const int *joints, const double *t)
1002{
1004}
1005
1010
const yarp::os::LogComponent & DYNAMIXELAX12FTDIDRIVER()
bool NOT_YET_IMPLEMENTED(const char *txt)
#define BOOL_EXIT_FAILURE
#define CT_GOAL_POSITION
#define INST_WRITE
#define CT_PRESENT_SPEED
#define CT_PRESENT_LOAD
#define AX12_BROADCAST_ID
defines for AX12 general ids
@ OVERHEATING_ERROR
#define INST_READ
#define MOTION_COMPLETION_TOLERANCE
#define CT_MAX_TORQUE
#define CT_PRESENT_POSITION
#define ReturnValue_ok
Definition ReturnValue.h:82
#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.
A simple collection of objects that can be described and transmitted in a portable way.
Definition Bottle.h:64
A mini-server for performing network communication in the background.
A base class for nested structures that can be searched.
Definition Searchable.h:31
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).
Definition Value.h:44
#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.