00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include "rx28.h"
00025
00026 #include <core/exceptions/system.h>
00027 #include <core/exceptions/software.h>
00028
00029 #include <utils/math/angle.h>
00030
00031 #include <sys/ioctl.h>
00032 #include <stdio.h>
00033 #include <sys/time.h>
00034 #include <termios.h>
00035 #include <fcntl.h>
00036 #include <errno.h>
00037 #include <unistd.h>
00038 #include <cstring>
00039 #include <cstdlib>
00040 #include <cstdarg>
00041
00042 const unsigned char RobotisRX28::BROADCAST_ID = 0xfe;
00043 const unsigned int RobotisRX28::MAX_POSITION = 0x3ff;
00044 const unsigned int RobotisRX28::CENTER_POSITION = 0x1ff;
00045 const unsigned int RobotisRX28::MAX_SPEED = 0x3ff;
00046 const float RobotisRX28::MAX_ANGLE_DEG = 300;
00047 const float RobotisRX28::MAX_ANGLE_RAD = fawkes::deg2rad(RobotisRX28::MAX_ANGLE_DEG);
00048 const float RobotisRX28::RAD_PER_POS_TICK = RobotisRX28::MAX_ANGLE_RAD / (float)(RobotisRX28::MAX_POSITION);
00049 const float RobotisRX28::POS_TICKS_PER_RAD = (float)(RobotisRX28::MAX_POSITION) / RobotisRX28::MAX_ANGLE_RAD;
00050 const float RobotisRX28::SEC_PER_60DEG_12V = 0.167;
00051 const float RobotisRX28::SEC_PER_60DEG_16V = 0.126;
00052
00053 const unsigned char RobotisRX28::SRL_RESPOND_NONE = 0;
00054 const unsigned char RobotisRX28::SRL_RESPOND_READ = 1;
00055 const unsigned char RobotisRX28::SRL_RESPOND_ALL = 2;
00056
00057 const unsigned char RobotisRX28::P_MODEL_NUMBER_L = 0;
00058 const unsigned char RobotisRX28::P_MODEL_NUMBER_H = 1;
00059 const unsigned char RobotisRX28::P_VERSION = 2;
00060 const unsigned char RobotisRX28::P_ID = 3;
00061 const unsigned char RobotisRX28::P_BAUD_RATE = 4;
00062 const unsigned char RobotisRX28::P_RETURN_DELAY_TIME = 5;
00063 const unsigned char RobotisRX28::P_CW_ANGLE_LIMIT_L = 6;
00064 const unsigned char RobotisRX28::P_CW_ANGLE_LIMIT_H = 7;
00065 const unsigned char RobotisRX28::P_CCW_ANGLE_LIMIT_L = 8;
00066 const unsigned char RobotisRX28::P_CCW_ANGLE_LIMIT_H = 9;
00067 const unsigned char RobotisRX28::P_SYSTEM_DATA2 = 10;
00068 const unsigned char RobotisRX28::P_LIMIT_TEMPERATURE = 11;
00069 const unsigned char RobotisRX28::P_DOWN_LIMIT_VOLTAGE = 12;
00070 const unsigned char RobotisRX28::P_UP_LIMIT_VOLTAGE = 13;
00071 const unsigned char RobotisRX28::P_MAX_TORQUE_L = 14;
00072 const unsigned char RobotisRX28::P_MAX_TORQUE_H = 15;
00073 const unsigned char RobotisRX28::P_RETURN_LEVEL = 16;
00074 const unsigned char RobotisRX28::P_ALARM_LED = 17;
00075 const unsigned char RobotisRX28::P_ALARM_SHUTDOWN = 18;
00076 const unsigned char RobotisRX28::P_OPERATING_MODE = 19;
00077 const unsigned char RobotisRX28::P_DOWN_CALIBRATION_L = 20;
00078 const unsigned char RobotisRX28::P_DOWN_CALIBRATION_H = 21;
00079 const unsigned char RobotisRX28::P_UP_CALIBRATION_L = 22;
00080 const unsigned char RobotisRX28::P_UP_CALIBRATION_H = 23;
00081
00082 const unsigned char RobotisRX28::P_TORQUE_ENABLE = 24;
00083 const unsigned char RobotisRX28::P_LED = 25;
00084 const unsigned char RobotisRX28::P_CW_COMPLIANCE_MARGIN = 26;
00085 const unsigned char RobotisRX28::P_CCW_COMPLIANCE_MARGIN = 27;
00086 const unsigned char RobotisRX28::P_CW_COMPLIANCE_SLOPE = 28;
00087 const unsigned char RobotisRX28::P_CCW_COMPLIANCE_SLOPE = 29;
00088 const unsigned char RobotisRX28::P_GOAL_POSITION_L = 30;
00089 const unsigned char RobotisRX28::P_GOAL_POSITION_H = 31;
00090 const unsigned char RobotisRX28::P_GOAL_SPEED_L = 32;
00091 const unsigned char RobotisRX28::P_GOAL_SPEED_H = 33;
00092 const unsigned char RobotisRX28::P_TORQUE_LIMIT_L = 34;
00093 const unsigned char RobotisRX28::P_TORQUE_LIMIT_H = 35;
00094 const unsigned char RobotisRX28::P_PRESENT_POSITION_L = 36;
00095 const unsigned char RobotisRX28::P_PRESENT_POSITION_H = 37;
00096 const unsigned char RobotisRX28::P_PRESENT_SPEED_L = 38;
00097 const unsigned char RobotisRX28::P_PRESENT_SPEED_H = 39;
00098 const unsigned char RobotisRX28::P_PRESENT_LOAD_L = 40;
00099 const unsigned char RobotisRX28::P_PRESENT_LOAD_H = 41;
00100 const unsigned char RobotisRX28::P_PRESENT_VOLTAGE = 42;
00101 const unsigned char RobotisRX28::P_PRESENT_TEMPERATURE = 43;
00102 const unsigned char RobotisRX28::P_REGISTERED_INSTRUCTION = 44;
00103 const unsigned char RobotisRX28::P_PAUSE_TIME = 45;
00104 const unsigned char RobotisRX28::P_MOVING = 46;
00105 const unsigned char RobotisRX28::P_LOCK = 47;
00106 const unsigned char RobotisRX28::P_PUNCH_L = 48;
00107 const unsigned char RobotisRX28::P_PUNCH_H = 49;
00108
00109
00110 const unsigned char RobotisRX28::INST_PING = 0x01;
00111 const unsigned char RobotisRX28::INST_READ = 0x02;
00112 const unsigned char RobotisRX28::INST_WRITE = 0x03;
00113 const unsigned char RobotisRX28::INST_REG_WRITE = 0x04;
00114 const unsigned char RobotisRX28::INST_ACTION = 0x05;
00115 const unsigned char RobotisRX28::INST_RESET = 0x06;
00116 const unsigned char RobotisRX28::INST_DIGITAL_RESET = 0x07;
00117 const unsigned char RobotisRX28::INST_SYSTEM_READ = 0x0C;
00118 const unsigned char RobotisRX28::INST_SYSTEM_WRITE = 0x0D;
00119 const unsigned char RobotisRX28::INST_SYNC_WRITE = 0x83;
00120 const unsigned char RobotisRX28::INST_SYNC_REG_WRITE = 0x84;
00121
00122 const unsigned char RobotisRX28::PACKET_OFFSET_ID = 2;
00123 const unsigned char RobotisRX28::PACKET_OFFSET_LENGTH = 3;
00124 const unsigned char RobotisRX28::PACKET_OFFSET_INST = 4;
00125 const unsigned char RobotisRX28::PACKET_OFFSET_PARAM = 5;
00126 const unsigned char RobotisRX28::PACKET_OFFSET_ERROR = 4;
00127
00128 using namespace std;
00129 using namespace fawkes;
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152 RobotisRX28::RobotisRX28(const char *device_file, unsigned int default_timeout_ms)
00153 {
00154 __default_timeout_ms = default_timeout_ms;
00155 __device_file = strdup(device_file);
00156 __fd = -1;
00157 __obuffer_length = 0;
00158 __ibuffer_length = 0;
00159 memset(__control_table, 0, RX28_MAX_NUM_SERVOS * RX28_CONTROL_TABLE_LENGTH);
00160 try {
00161 open();
00162 } catch (Exception &e) {
00163 free(__device_file);
00164 throw;
00165 }
00166 }
00167
00168
00169
00170 RobotisRX28::~RobotisRX28()
00171 {
00172 free(__device_file);
00173 }
00174
00175
00176 void
00177 RobotisRX28::open() {
00178
00179 struct termios param;
00180
00181 __fd = ::open(__device_file, O_NOCTTY | O_RDWR);
00182 if (__fd == -1) {
00183 throw CouldNotOpenFileException(__device_file, errno, "Cannot open device file");
00184 }
00185
00186 if (tcgetattr(__fd, ¶m) == -1) {
00187 Exception e(errno, "Getting the port parameters failed");
00188 ::close(__fd);
00189 __fd = -1;
00190 throw e;
00191 }
00192
00193 cfsetospeed(¶m, B57600);
00194 cfsetispeed(¶m, B57600);
00195
00196 param.c_lflag &= ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | ISIG | IEXTEN);
00197 param.c_iflag &= ~(INLCR | IGNCR | ICRNL | IGNBRK | PARMRK);
00198
00199
00200 param.c_iflag &= ~(IXON | IXOFF | IXANY);
00201 param.c_cflag &= ~CRTSCTS;
00202
00203 param.c_cflag |= (CREAD | CLOCAL);
00204
00205
00206 param.c_cflag &= ~CS5 & ~CS6 & ~CS7 & ~CS8;
00207 param.c_cflag |= CS8;
00208
00209
00210 param.c_cflag &= ~(PARENB | PARODD);
00211 param.c_iflag &= ~(INPCK | ISTRIP);
00212
00213
00214 param.c_cflag &= ~CSTOPB;
00215
00216
00217 param.c_oflag &= ~OPOST;
00218
00219 param.c_cc[VMIN] = 1;
00220 param.c_cc[VTIME] = 0;
00221
00222 tcflush(__fd, TCIOFLUSH);
00223
00224 if (tcsetattr(__fd, TCSANOW, ¶m) != 0) {
00225 Exception e(errno, "Setting the port parameters failed");
00226 ::close(__fd);
00227 __fd = -1;
00228 throw e;
00229 }
00230
00231
00232
00233
00234 #ifdef TIMETRACKER_VISCA
00235 tracker = new TimeTracker();
00236 track_file.open("tracker_visca.txt");
00237 ttcls_pantilt_get_send = tracker->addClass("getPanTilt: send");
00238 ttcls_pantilt_get_read = tracker->addClass("getPanTilt: read");
00239 ttcls_pantilt_get_handle = tracker->addClass("getPanTilt: handling responses");
00240 ttcls_pantilt_get_interpret = tracker->addClass("getPanTilt: interpreting");
00241 #endif
00242
00243
00244 }
00245
00246
00247
00248 void
00249 RobotisRX28::close()
00250 {
00251 if (__fd >= 0) {
00252 ::close(__fd);
00253 __fd = -1;
00254 }
00255 }
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265 unsigned char
00266 RobotisRX28::calc_checksum(const unsigned char id, const unsigned char instruction,
00267 const unsigned char *params, const unsigned char plength)
00268 {
00269 unsigned int checksum = id + instruction + plength+2;
00270 for (unsigned char i = 0; i < plength; ++i) {
00271 checksum += params[i];
00272 }
00273
00274 return ~(checksum & 0xFF);
00275 }
00276
00277
00278
00279
00280
00281
00282
00283
00284 void
00285 RobotisRX28::send(const unsigned char id, const unsigned char instruction,
00286 const unsigned char *params, const unsigned char plength)
00287 {
00288
00289 __obuffer[0] = 0xFF;
00290 __obuffer[1] = 0xFF;
00291 __obuffer[PACKET_OFFSET_ID] = id;
00292 __obuffer[PACKET_OFFSET_LENGTH] = plength+2;
00293 __obuffer[PACKET_OFFSET_INST] = instruction;
00294
00295 unsigned int checksum = id + plength+2;
00296
00297 for (unsigned char i = 0; i < plength; ++i) {
00298 __obuffer[PACKET_OFFSET_PARAM + i] = params[i];
00299 checksum += params[i];
00300 }
00301
00302
00303 __obuffer[3 + plength+2] = calc_checksum(id, instruction, params, plength);
00304 __obuffer_length = plength+2 + 4 ;
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314 int written = write(__fd, __obuffer, __obuffer_length);
00315
00316
00317
00318 int readd __attribute__((unused)) = read(__fd, __ibuffer, __obuffer_length);
00319
00320
00321 if ( written < 0 ) {
00322 throw Exception(errno, "Failed to write RX28 packet %x for %x", instruction, id);
00323 } else if (written < __obuffer_length) {
00324 throw Exception("Failed to write RX28 packet %x for %x, only %d of %d bytes sent",
00325 instruction, id, written, __obuffer_length);
00326 }
00327 }
00328
00329
00330
00331
00332
00333 void
00334 RobotisRX28::recv(unsigned int timeout_ms)
00335 {
00336 timeval timeout = {0, (timeout_ms == 0xFFFFFFFF ? __default_timeout_ms : timeout_ms) * 1000};
00337
00338 fd_set read_fds;
00339 FD_ZERO(&read_fds);
00340 FD_SET(__fd, &read_fds);
00341
00342 int rv = 0;
00343 rv = select(__fd + 1, &read_fds, NULL, NULL, &timeout);
00344
00345 if ( rv == -1 ) {
00346 throw Exception(errno, "Select on FD failed");
00347 } else if ( rv == 0 ) {
00348
00349 throw TimeoutException("Timeout reached while waiting for incoming RX28 data");
00350 }
00351
00352 __ibuffer_length = 0;
00353
00354
00355 int bytes_read = 0;
00356 while (bytes_read < 6) {
00357
00358 bytes_read += read(__fd, __ibuffer + bytes_read, 6 - bytes_read);
00359
00360
00361
00362
00363
00364
00365
00366 }
00367 if (bytes_read < 6) {
00368 throw Exception("Failed to read packet header");
00369 }
00370 unsigned char plength = __ibuffer[PACKET_OFFSET_LENGTH] - 2;
00371
00372 if (plength > 0) {
00373 bytes_read = 0;
00374 while (bytes_read < plength) {
00375 bytes_read += read(__fd, &__ibuffer[6] + bytes_read, plength - bytes_read);
00376 }
00377 if (bytes_read < plength) {
00378 throw Exception("Failed to read packet data");
00379 }
00380 }
00381
00382 __ibuffer_length = plength+2 + 4;
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392 unsigned char checksum = calc_checksum(__ibuffer[PACKET_OFFSET_ID],
00393 __ibuffer[PACKET_OFFSET_INST],
00394 &__ibuffer[PACKET_OFFSET_PARAM], plength);
00395 if (checksum != __ibuffer[plength + 5]) {
00396 throw Exception("Checksum error while receiving packeg, expected %d, got %d",
00397 checksum, __ibuffer[plength + 5]);
00398 }
00399
00400 __ibuffer_length = plength+2 + 4;
00401 }
00402
00403
00404
00405
00406
00407 bool
00408 RobotisRX28::data_available()
00409 {
00410 int num_bytes = 0;
00411 ioctl(__fd, FIONREAD, &num_bytes);
00412 return (num_bytes > 0);
00413 }
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434 RobotisRX28::DeviceList
00435 RobotisRX28::discover(unsigned int timeout_ms)
00436 {
00437 DeviceList rv;
00438
00439
00440 send(BROADCAST_ID, INST_PING, NULL, 0);
00441
00442 for (unsigned int i = 0; i < RX28_MAX_NUM_SERVOS; ++i) {
00443 try {
00444 recv(timeout_ms);
00445 rv.push_back(__ibuffer[PACKET_OFFSET_ID]);
00446 } catch (TimeoutException &e) {
00447
00448 break;
00449 }
00450 }
00451
00452
00453 for (DeviceList::iterator i = rv.begin(); i != rv.end(); ++i) {
00454 try {
00455 read_table_values(*i);
00456 } catch (Exception &e) {
00457 e.append("Failed to receive control table for servo %u", *i);
00458 throw;
00459 }
00460 }
00461
00462 return rv;
00463 }
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473 bool
00474 RobotisRX28::ping(unsigned char id, unsigned int timeout_ms)
00475 {
00476 assert_valid_id(id);
00477 try {
00478 send(id, INST_PING, NULL, 0);
00479 recv(timeout_ms);
00480 return true;
00481 } catch (Exception &e) {
00482 e.print_trace();
00483 return false;
00484 }
00485 }
00486
00487
00488
00489
00490
00491
00492
00493 void
00494 RobotisRX28::read_table_values(unsigned char id)
00495 {
00496 start_read_table_values(id);
00497 finish_read_table_values();
00498 }
00499
00500
00501
00502
00503
00504
00505
00506
00507
00508 void
00509 RobotisRX28::start_read_table_values(unsigned char id)
00510 {
00511 assert_valid_id(id);
00512 unsigned char param[2];
00513 param[0] = 0x00;
00514 param[1] = RX28_CONTROL_TABLE_LENGTH;
00515
00516 send(id, INST_READ, param, 2);
00517 }
00518
00519
00520
00521
00522
00523
00524
00525 void
00526 RobotisRX28::finish_read_table_values()
00527 {
00528 recv();
00529
00530 if (__ibuffer_length != 5 + RX28_CONTROL_TABLE_LENGTH + 1) {
00531 throw Exception("Input buffer of invalid size");
00532 }
00533 memcpy(__control_table[__ibuffer[PACKET_OFFSET_ID]],
00534 &__ibuffer[PACKET_OFFSET_PARAM], RX28_CONTROL_TABLE_LENGTH);
00535 }
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546 void
00547 RobotisRX28::read_table_value(unsigned char id,
00548 unsigned char addr, unsigned char read_length)
00549 {
00550 assert_valid_id(id);
00551
00552 unsigned char param[2];
00553 param[0] = addr;
00554 param[1] = read_length;
00555
00556 send(id, INST_READ, param, 2);
00557 recv();
00558
00559 if (__ibuffer_length != (5 + read_length + 1)) {
00560 throw Exception("Input buffer not of expected size, expected %u, got %u",
00561 (5 + read_length + 1), __ibuffer_length);
00562 }
00563
00564 for (unsigned int i = 0; i < read_length; ++i) {
00565 __control_table[id][addr + i] = __ibuffer[PACKET_OFFSET_PARAM + i];
00566 }
00567 }
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577 void
00578 RobotisRX28::write_table_value(unsigned char id, unsigned char addr,
00579 unsigned int value, bool double_byte)
00580 {
00581 unsigned char param[3];
00582 param[0] = addr;
00583 param[1] = value & 0xFF;
00584 param[2] = (value >> 8) & 0xFF;
00585
00586 try {
00587 send(id, INST_WRITE, param, double_byte ? 3 : 2);
00588
00589 if (id == BROADCAST_ID) {
00590 for (unsigned int i = 0; i < RX28_MAX_NUM_SERVOS; ++i) {
00591 __control_table[i][addr] = param[1];
00592 if (double_byte) __control_table[i][addr + 1] = param[2];
00593 }
00594 } else {
00595 __control_table[id][addr] = param[1];
00596 if (double_byte) __control_table[id][addr + 1] = param[2];
00597 }
00598
00599 if ((id != BROADCAST_ID) && responds_all(id)) recv();
00600 } catch (Exception &e) {
00601 e.print_trace();
00602 throw;
00603 }
00604 }
00605
00606
00607
00608
00609
00610
00611
00612
00613 void
00614 RobotisRX28::write_table_values(unsigned char id, unsigned char start_addr,
00615 unsigned char *values, unsigned int num_values)
00616 {
00617 unsigned char param[num_values + 1];
00618 param[0] = start_addr;
00619 for (unsigned int i = 0; i < num_values; ++i) {
00620 param[i + 1] = values[i];
00621 }
00622
00623 try {
00624 send(id, INST_WRITE, param, num_values + 1);
00625
00626 if (id == BROADCAST_ID) {
00627 for (unsigned int i = 0; i < RX28_MAX_NUM_SERVOS; ++i) {
00628 for (unsigned int j = 0; j < num_values; ++j) {
00629 __control_table[i][start_addr + j] = values[j];
00630 }
00631 }
00632 } else {
00633 for (unsigned int j = 0; j < num_values; ++j) {
00634 __control_table[id][start_addr + j] = values[j];
00635 }
00636 }
00637
00638 if ((id != BROADCAST_ID) && responds_all(id)) recv();
00639 } catch (Exception &e) {
00640 e.print_trace();
00641 throw;
00642 }
00643 }
00644
00645
00646
00647
00648
00649
00650
00651 void
00652 RobotisRX28::assert_valid_id(unsigned char id)
00653 {
00654 if (id == BROADCAST_ID) {
00655 throw Exception("Data can only be queried for a specific servo");
00656 } else if (id >= RX28_MAX_NUM_SERVOS) {
00657 throw OutOfBoundsException("Servo ID out of bounds", id, 0, RX28_MAX_NUM_SERVOS);
00658 }
00659 }
00660
00661
00662
00663
00664
00665
00666
00667 unsigned int
00668 RobotisRX28::merge_twobyte_value(unsigned int id,
00669 unsigned char ind_l, unsigned char ind_h)
00670 {
00671 unsigned int rv = (__control_table[id][ind_h] & 0xFF) << 8;
00672 rv |= __control_table[id][ind_l] & 0xFF;
00673 return rv;
00674 }
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685 unsigned int
00686 RobotisRX28::get_value(unsigned int id, bool refresh,
00687 unsigned int ind_l, unsigned int ind_h)
00688 {
00689 assert_valid_id(id);
00690
00691 if (refresh) read_table_value(id, ind_l, (ind_h == 0xFFFFFFFF ? 1 : 2));
00692
00693 if (ind_h == 0xFFFFFFFF) {
00694 return __control_table[id][ind_l];
00695 } else {
00696 return merge_twobyte_value(id, ind_l, ind_h);
00697 }
00698 }
00699
00700
00701
00702
00703
00704
00705 unsigned int
00706 RobotisRX28::get_model(unsigned char id, bool refresh)
00707 {
00708 return get_value(id, refresh, P_MODEL_NUMBER_L, P_MODEL_NUMBER_H);
00709 }
00710
00711
00712
00713
00714
00715
00716
00717 unsigned int
00718 RobotisRX28::get_position(unsigned char id, bool refresh)
00719 {
00720 return get_value(id, refresh, P_PRESENT_POSITION_L, P_PRESENT_POSITION_H);
00721 }
00722
00723
00724
00725
00726
00727
00728
00729 unsigned char
00730 RobotisRX28::get_firmware_version(unsigned char id, bool refresh)
00731 {
00732 return get_value(id, refresh, P_VERSION);
00733 }
00734
00735
00736
00737
00738
00739
00740
00741 unsigned char
00742 RobotisRX28::get_baudrate(unsigned char id, bool refresh)
00743 {
00744 return get_value(id, refresh, P_BAUD_RATE);
00745 }
00746
00747
00748
00749
00750
00751
00752
00753 unsigned char
00754 RobotisRX28::get_delay_time(unsigned char id, bool refresh)
00755 {
00756 return get_value(id, refresh, P_RETURN_DELAY_TIME);
00757 }
00758
00759
00760
00761
00762
00763
00764
00765
00766 void
00767 RobotisRX28::get_angle_limits(unsigned char id,
00768 unsigned int &cw_limit, unsigned int &ccw_limit,
00769 bool refresh)
00770 {
00771 cw_limit = get_value(id, refresh, P_CW_ANGLE_LIMIT_L, P_CW_ANGLE_LIMIT_H);
00772 ccw_limit = get_value(id, refresh, P_CCW_ANGLE_LIMIT_L, P_CCW_ANGLE_LIMIT_H);
00773 }
00774
00775
00776
00777
00778
00779
00780
00781 unsigned char
00782 RobotisRX28::get_temperature_limit(unsigned char id, bool refresh)
00783 {
00784 return get_value(id, refresh, P_LIMIT_TEMPERATURE);
00785 }
00786
00787
00788
00789
00790
00791
00792
00793
00794 void
00795 RobotisRX28::get_voltage_limits(unsigned char id,
00796 unsigned char &low, unsigned char &high,
00797 bool refresh)
00798 {
00799 low = get_value(id, refresh, P_DOWN_LIMIT_VOLTAGE);
00800 high = get_value(id, refresh, P_UP_LIMIT_VOLTAGE);
00801 }
00802
00803
00804
00805
00806
00807
00808
00809 unsigned int
00810 RobotisRX28::get_max_torque(unsigned char id, bool refresh)
00811 {
00812 return get_value(id, refresh, P_MAX_TORQUE_L, P_MAX_TORQUE_H);
00813 }
00814
00815
00816
00817
00818
00819
00820
00821 unsigned char
00822 RobotisRX28::get_status_return_level(unsigned char id, bool refresh)
00823 {
00824 return get_value(id, refresh, P_RETURN_LEVEL);
00825 }
00826
00827
00828
00829
00830
00831
00832
00833 unsigned char
00834 RobotisRX28::get_alarm_led(unsigned char id, bool refresh)
00835 {
00836 return get_value(id, refresh, P_ALARM_LED);
00837 }
00838
00839
00840
00841
00842
00843
00844
00845 unsigned char
00846 RobotisRX28::get_alarm_shutdown(unsigned char id, bool refresh)
00847 {
00848 return get_value(id, refresh, P_ALARM_SHUTDOWN);
00849 }
00850
00851
00852
00853
00854
00855
00856
00857
00858 void
00859 RobotisRX28::get_calibration(unsigned char id,
00860 unsigned int &down_calib, unsigned int &up_calib,
00861 bool refresh)
00862 {
00863 down_calib = get_value(id, refresh, P_DOWN_CALIBRATION_L, P_DOWN_CALIBRATION_H);
00864 up_calib = get_value(id, refresh, P_UP_CALIBRATION_L, P_UP_CALIBRATION_H);
00865 }
00866
00867
00868
00869
00870
00871
00872
00873 bool
00874 RobotisRX28::is_torque_enabled(unsigned char id, bool refresh)
00875 {
00876 return (get_value(id, refresh, P_TORQUE_ENABLE) == 1);
00877 }
00878
00879
00880
00881
00882
00883
00884
00885 bool
00886 RobotisRX28::is_led_enabled(unsigned char id, bool refresh)
00887 {
00888 return (get_value(id, refresh, P_LED) == 1);
00889 }
00890
00891
00892
00893
00894
00895
00896
00897
00898
00899
00900 void
00901 RobotisRX28::get_compliance_values(unsigned char id,
00902 unsigned char &cw_margin, unsigned char &cw_slope,
00903 unsigned char &ccw_margin, unsigned char &ccw_slope,
00904 bool refresh)
00905 {
00906 cw_margin = get_value(id, refresh, P_CW_COMPLIANCE_MARGIN);
00907 cw_slope = get_value(id, refresh, P_CW_COMPLIANCE_SLOPE);
00908 ccw_margin = get_value(id, refresh, P_CCW_COMPLIANCE_MARGIN);
00909 ccw_slope = get_value(id, refresh, P_CCW_COMPLIANCE_SLOPE);
00910 }
00911
00912
00913
00914
00915
00916
00917
00918 unsigned int
00919 RobotisRX28::get_goal_position(unsigned char id, bool refresh)
00920 {
00921 return get_value(id, refresh, P_GOAL_POSITION_L, P_GOAL_POSITION_H);
00922 }
00923
00924
00925
00926
00927
00928
00929
00930 unsigned int
00931 RobotisRX28::get_goal_speed(unsigned char id, bool refresh)
00932 {
00933 return get_value(id, refresh, P_GOAL_SPEED_L, P_GOAL_SPEED_H);
00934 }
00935
00936
00937
00938
00939
00940
00941
00942 float
00943 RobotisRX28::get_max_supported_speed(unsigned char id, bool refresh)
00944 {
00945 float voltage = get_voltage(id, refresh) / 10.0;
00946
00947 if ((voltage < 12.0) || (voltage > 16.0)) {
00948 throw OutOfBoundsException("Voltage is outside of specs", voltage, 12.0, 16.0);
00949 }
00950
00951 float sec_per_deg_12V = SEC_PER_60DEG_12V / 60.0;
00952 float sec_per_deg_16V = SEC_PER_60DEG_16V / 60.0;
00953
00954 float range_sec_per_deg = sec_per_deg_12V - sec_per_deg_16V;
00955 float pos = voltage - 12.0;
00956
00957 float sec_per_deg = sec_per_deg_16V + pos * range_sec_per_deg;
00958 float deg_per_sec = 1.0 / sec_per_deg;
00959
00960 return deg2rad(deg_per_sec);
00961 }
00962
00963
00964
00965
00966
00967
00968
00969 unsigned int
00970 RobotisRX28::get_torque_limit(unsigned char id, bool refresh)
00971 {
00972 return get_value(id, refresh, P_TORQUE_LIMIT_L, P_TORQUE_LIMIT_H);
00973 }
00974
00975
00976
00977
00978
00979
00980
00981 unsigned int
00982 RobotisRX28::get_speed(unsigned char id, bool refresh)
00983 {
00984 return get_value(id, refresh, P_PRESENT_SPEED_L, P_PRESENT_SPEED_H);
00985 }
00986
00987
00988
00989
00990
00991
00992
00993 unsigned int
00994 RobotisRX28::get_load(unsigned char id, bool refresh)
00995 {
00996 return get_value(id, refresh, P_PRESENT_LOAD_L, P_PRESENT_LOAD_H);
00997 }
00998
00999
01000
01001
01002
01003
01004
01005 unsigned char
01006 RobotisRX28::get_voltage(unsigned char id, bool refresh)
01007 {
01008 return get_value(id, refresh, P_PRESENT_VOLTAGE);
01009 }
01010
01011
01012
01013
01014
01015
01016
01017 unsigned char
01018 RobotisRX28::get_temperature(unsigned char id, bool refresh)
01019 {
01020 return get_value(id, refresh, P_PRESENT_TEMPERATURE);
01021 }
01022
01023
01024
01025
01026
01027
01028
01029 bool
01030 RobotisRX28::is_moving(unsigned char id, bool refresh)
01031 {
01032 return (get_value(id, refresh, P_MOVING) == 1);
01033 }
01034
01035
01036
01037
01038
01039
01040
01041 bool
01042 RobotisRX28::is_locked(unsigned char id, bool refresh)
01043 {
01044 return (get_value(id, refresh, P_LOCK) == 1);
01045 }
01046
01047
01048
01049
01050
01051
01052
01053 unsigned int
01054 RobotisRX28::get_punch(unsigned char id, bool refresh)
01055 {
01056 return get_value(id, refresh, P_PUNCH_L, P_PUNCH_H);
01057 }
01058
01059
01060
01061
01062
01063
01064 void
01065 RobotisRX28::set_id(unsigned char id, unsigned char new_id)
01066 {
01067 write_table_value(id, P_ID, new_id);
01068 }
01069
01070
01071
01072
01073
01074
01075 void
01076 RobotisRX28::set_baudrate(unsigned char id, unsigned char baudrate)
01077 {
01078 write_table_value(id, P_BAUD_RATE, baudrate);
01079 }
01080
01081
01082
01083
01084
01085
01086 void
01087 RobotisRX28::set_return_delay_time(unsigned char id, unsigned char return_delay_time)
01088 {
01089 write_table_value(id, P_RETURN_DELAY_TIME, return_delay_time);
01090 }
01091
01092
01093
01094
01095
01096
01097
01098 void
01099 RobotisRX28::set_angle_limits(unsigned char id,
01100 unsigned int cw_limit, unsigned int ccw_limit)
01101 {
01102 write_table_value(id, P_CW_ANGLE_LIMIT_L, cw_limit, true);
01103 write_table_value(id, P_CCW_ANGLE_LIMIT_L, ccw_limit, true);
01104 }
01105
01106
01107
01108
01109
01110
01111 void
01112 RobotisRX28::set_temperature_limit(unsigned char id, unsigned char temp_limit)
01113 {
01114 write_table_value(id, P_LIMIT_TEMPERATURE, temp_limit);
01115 }
01116
01117
01118
01119
01120
01121
01122
01123 void
01124 RobotisRX28::set_voltage_limits(unsigned char id, unsigned char low, unsigned char high)
01125 {
01126 unsigned char param[2];
01127 param[0] = low;
01128 param[1] = high;
01129 write_table_values(id, P_DOWN_LIMIT_VOLTAGE, param, 2);
01130 }
01131
01132
01133
01134
01135
01136
01137 void
01138 RobotisRX28::set_max_torque(unsigned char id, unsigned int max_torque)
01139 {
01140 write_table_value(id, P_MAX_TORQUE_L, max_torque, true);
01141 }
01142
01143
01144
01145
01146
01147
01148
01149 void
01150 RobotisRX28::set_status_return_level(unsigned char id, unsigned char status_return_level)
01151 {
01152 write_table_value(id, P_RETURN_LEVEL, status_return_level);
01153 }
01154
01155
01156
01157
01158
01159
01160 void
01161 RobotisRX28::set_alarm_led(unsigned char id, unsigned char alarm_led)
01162 {
01163 write_table_value(id, P_ALARM_LED, alarm_led);
01164 }
01165
01166
01167
01168
01169
01170
01171 void
01172 RobotisRX28::set_alarm_shutdown(unsigned char id, unsigned char alarm_shutdown)
01173 {
01174 write_table_value(id, P_ALARM_SHUTDOWN, alarm_shutdown);
01175 }
01176
01177
01178
01179
01180
01181
01182
01183 void
01184 RobotisRX28::set_torque_enabled(unsigned char id, bool enabled )
01185 {
01186 write_table_value(id, P_TORQUE_ENABLE, enabled ? 1 : 0);
01187 }
01188
01189
01190
01191
01192
01193
01194
01195
01196
01197 void
01198 RobotisRX28::set_torques_enabled(bool enabled, unsigned char num_servos, ...)
01199 {
01200 if (num_servos > 120) {
01201
01202 throw Exception("You cannot set more than 120 servos at once");
01203 }
01204
01205 va_list arg;
01206 va_start(arg, num_servos);
01207
01208 unsigned int plength = 2 * num_servos + 2;
01209 unsigned char param[plength];
01210 param[0] = P_TORQUE_ENABLE;
01211 param[1] = 1;
01212 for (unsigned int i = 0; i < num_servos; ++i) {
01213 unsigned char id = va_arg(arg, unsigned int);
01214 param[2 + i * 2] = id;
01215 param[2 + i * 2 + 1] = enabled ? 1 : 0;
01216 }
01217 va_end(arg);
01218
01219 send(BROADCAST_ID, INST_SYNC_WRITE, param, plength);
01220 }
01221
01222
01223
01224
01225
01226
01227 void
01228 RobotisRX28::set_led_enabled(unsigned char id, bool led_enabled )
01229 {
01230 write_table_value(id, P_LED, led_enabled ? 1 : 0);
01231 }
01232
01233
01234
01235
01236
01237
01238
01239
01240
01241 void
01242 RobotisRX28::set_compliance_values(unsigned char id,
01243 unsigned char cw_margin, unsigned char cw_slope,
01244 unsigned char ccw_margin, unsigned char ccw_slope)
01245 {
01246 unsigned char param[4];
01247 param[0] = cw_margin;
01248 param[1] = ccw_margin;
01249 param[2] = cw_slope;
01250 param[3] = ccw_slope;
01251 write_table_values(id, P_CW_COMPLIANCE_MARGIN, param, 4);
01252 }
01253
01254
01255
01256
01257
01258
01259
01260 void
01261 RobotisRX28::set_goal_speed(unsigned char id, unsigned int goal_speed)
01262 {
01263 write_table_value(id, P_GOAL_SPEED_L, goal_speed, true);
01264 }
01265
01266
01267
01268
01269
01270
01271
01272 void
01273 RobotisRX28::set_goal_speeds(unsigned int num_servos, ...)
01274 {
01275 if (num_servos > 83) {
01276
01277 throw Exception("You cannot set more than 83 speeds at once");
01278 }
01279
01280 va_list arg;
01281 va_start(arg, num_servos);
01282
01283 unsigned int plength = 3 * num_servos + 2;
01284 unsigned char param[plength];
01285 param[0] = P_GOAL_SPEED_L;
01286 param[1] = 2;
01287 for (unsigned int i = 0; i < num_servos; ++i) {
01288 unsigned char id = va_arg(arg, unsigned int);
01289 unsigned int value = va_arg(arg, unsigned int);
01290 printf("Servo speed %u to %u\n", id, value);
01291 param[2 + i * 3] = id;
01292 param[2 + i * 3 + 1] = (value & 0xFF);
01293 param[2 + i * 3 + 2] = (value >> 8) & 0xFF;
01294 }
01295 va_end(arg);
01296
01297 send(BROADCAST_ID, INST_SYNC_WRITE, param, plength);
01298 }
01299
01300
01301
01302
01303
01304
01305 void
01306 RobotisRX28::set_torque_limit(unsigned char id, unsigned int torque_limit)
01307 {
01308 write_table_value(id, P_TORQUE_LIMIT_L, torque_limit, true);
01309 }
01310
01311
01312
01313
01314
01315
01316 void
01317 RobotisRX28::set_punch(unsigned char id, unsigned int punch)
01318 {
01319 write_table_value(id, P_PUNCH_L, punch, true);
01320 }
01321
01322
01323
01324
01325
01326
01327
01328 void
01329 RobotisRX28::lock_config(unsigned char id)
01330 {
01331 write_table_value(id, P_LOCK, 1);
01332 }
01333
01334
01335
01336
01337
01338
01339
01340 void
01341 RobotisRX28::goto_position(unsigned char id, unsigned int value)
01342 {
01343 write_table_value(id, P_GOAL_POSITION_L, value, true);
01344 }
01345
01346
01347
01348
01349
01350
01351
01352
01353 void
01354 RobotisRX28::goto_positions(unsigned int num_servos, ...)
01355 {
01356 if (num_servos > 83) {
01357
01358 throw Exception("You cannot set more than 83 servos at once");
01359 }
01360
01361 va_list arg;
01362 va_start(arg, num_servos);
01363
01364 unsigned int plength = 3 * num_servos + 2;
01365 unsigned char param[plength];
01366 param[0] = P_GOAL_POSITION_L;
01367 param[1] = 2;
01368 for (unsigned int i = 0; i < num_servos; ++i) {
01369 unsigned char id = va_arg(arg, unsigned int);
01370 unsigned int value = va_arg(arg, unsigned int);
01371 param[2 + i * 3] = id;
01372 param[2 + i * 3 + 1] = (value & 0xFF);
01373 param[2 + i * 3 + 2] = (value >> 8) & 0xFF;
01374 }
01375 va_end(arg);
01376
01377 send(BROADCAST_ID, INST_SYNC_WRITE, param, plength);
01378 }
01379