dynamixel_io.cpp
Go to the documentation of this file.
00001 /*
00002     Copyright (c) 2011, Antons Rebguns <email>
00003     All rights reserved.
00004 
00005     Redistribution and use in source and binary forms, with or without
00006     modification, are permitted provided that the following conditions are met:
00007         * Redistributions of source code must retain the above copyright
00008         notice, this list of conditions and the following disclaimer.
00009         * Redistributions in binary form must reproduce the above copyright
00010         notice, this list of conditions and the following disclaimer in the
00011         documentation and/or other materials provided with the distribution.
00012         * Neither the name of the <organization> nor the
00013         names of its contributors may be used to endorse or promote products
00014         derived from this software without specific prior written permission.
00015 
00016     THIS SOFTWARE IS PROVIDED BY Antons Rebguns <email> ''AS IS'' AND ANY
00017     EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018     WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019     DISCLAIMED. IN NO EVENT SHALL Antons Rebguns <email> BE LIABLE FOR ANY
00020     DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021     (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022     LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023     ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024     (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025     SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include <time.h>
00029 #include <pthread.h>
00030 #include <stdint.h>
00031 #include <stdio.h>
00032 
00033 #include <sstream>
00034 #include <map>
00035 #include <set>
00036 #include <string>
00037 #include <vector>
00038 
00039 #include <gearbox/flexiport/flexiport.h>
00040 
00041 #include <dynamixel_hardware_interface/dynamixel_const.h>
00042 #include <dynamixel_hardware_interface/dynamixel_io.h>
00043 
00044 namespace dynamixel_hardware_interface
00045 {
00046 
00047 DynamixelIO::DynamixelIO(std::string device="/dev/ttyUSB0",
00048                          std::string baud="1000000")
00049 {
00050     std::map<std::string, std::string> options;
00051     options["type"] = "serial";
00052     //options["timeout"] = "0.0001";
00053     options["alwaysopen"] = "true";
00054     options["device"] = device;
00055     options["baud"] = baud;
00056     
00057     read_count = 0;
00058     read_error_count = 0;
00059     last_reset_sec = 0.0;
00060 
00061     pthread_mutex_init(&serial_mutex_, NULL);
00062     port_ = flexiport::CreatePort(options);
00063     
00064     // 100 microseconds = 0.1 milliseconds
00065     flexiport::Timeout t(0, 100);
00066     port_->SetTimeout(t);
00067 }
00068 
00069 DynamixelIO::~DynamixelIO()
00070 {
00071     port_->Close();
00072     delete port_;
00073     pthread_mutex_destroy(&serial_mutex_);
00074     
00075     std::map<int, DynamixelData*>::iterator it;
00076     for (it = cache_.begin(); it != cache_.end(); ++it)
00077     {
00078         delete it->second;
00079     }
00080 }
00081 
00082 const DynamixelData* DynamixelIO::getCachedParameters(int servo_id)
00083 {
00084     DynamixelData* dd = findCachedParameters(servo_id);
00085     if (!updateCachedParameters(servo_id, dd)) { return NULL; }
00086     return dd;
00087 }
00088 
00089 bool DynamixelIO::ping(int servo_id)
00090 {
00091     // Instruction, checksum
00092     uint8_t length = 2;
00093 
00094     // Check Sum = ~ (ID + LENGTH + INSTRUCTION + PARAM_1 + ... + PARAM_N)
00095     // If the calculated value is > 255, the lower byte is the check sum.
00096     uint8_t checksum = 0xFF - ( (servo_id + length + DXL_PING) % 256 );
00097 
00098     // packet: FF  FF  ID LENGTH INSTRUCTION PARAM_1 ... CHECKSUM
00099     const uint8_t packet_length = 6;
00100     uint8_t packet[packet_length] = { 0xFF, 0xFF, servo_id, length, DXL_PING, checksum };
00101 
00102     std::vector<uint8_t> response;
00103 
00104     pthread_mutex_lock(&serial_mutex_);
00105     bool success = writePacket(packet, packet_length);
00106     if (success) { success = readResponse(response); }
00107     pthread_mutex_unlock(&serial_mutex_);
00108     
00109     if (success)
00110     {
00111         DynamixelData* dd = findCachedParameters(servo_id);
00112         updateCachedParameters(servo_id, dd);
00113         
00114         checkForErrors(servo_id, response[4], "ping");
00115         connected_motors_.insert(servo_id);
00116     }
00117     
00118     return success;
00119 }
00120 
00121 bool DynamixelIO::resetOverloadError(int servo_id)
00122 {
00123     if (setTorqueEnable(servo_id, false))
00124     {
00125         DynamixelData* dd = findCachedParameters(servo_id);
00126         
00127         if (setTorqueLimit(servo_id, dd->max_torque))
00128         {
00129             setLed(servo_id, false);
00130             dd->shutdown_error_time = 0.0;
00131             
00132             return true;
00133         }
00134     }
00135     
00136     return false;
00137 }
00138 
00139 bool DynamixelIO::getModelNumber(int servo_id, uint16_t& model_number)
00140 {
00141     std::vector<uint8_t> response;
00142 
00143     if (read(servo_id, DXL_MODEL_NUMBER_L, 2, response))
00144     {
00145         checkForErrors(servo_id, response[4], "getModelNumber");
00146         model_number = response[5] + (response[6] << 8);
00147         return true;
00148     }
00149 
00150     return false;
00151 }
00152 
00153 bool DynamixelIO::getFirmwareVersion(int servo_id, uint8_t& firmware_version)
00154 {
00155     std::vector<uint8_t> response;
00156 
00157     if (read(servo_id, DXL_FIRMWARE_VERSION, 1, response))
00158     {
00159         checkForErrors(servo_id, response[4], "getFirmwareVersion");
00160         firmware_version = response[5];
00161         return true;
00162     }
00163 
00164     return false;
00165 }
00166 
00167 bool DynamixelIO::getBaudRate(int servo_id, uint8_t& baud_rate)
00168 {
00169     std::vector<uint8_t> response;
00170     
00171     if (read(servo_id, DXL_BAUD_RATE, 1, response))
00172     {
00173         checkForErrors(servo_id, response[4], "getBaudRate");
00174         baud_rate = response[5];
00175         return true;
00176     }
00177     
00178     return false;
00179 }
00180 
00181 bool DynamixelIO::getReturnDelayTime(int servo_id, uint8_t& return_delay_time)
00182 {
00183     std::vector<uint8_t> response;
00184 
00185     if (read(servo_id, DXL_RETURN_DELAY_TIME, 1, response))
00186     {
00187         checkForErrors(servo_id, response[4], "getReturnDelayTime");
00188         return_delay_time = response[5];
00189         return true;
00190     }
00191 
00192     return false;
00193 }
00194 
00195 bool DynamixelIO::getAngleLimits(int servo_id, uint16_t& cw_angle_limit, uint16_t& ccw_angle_limit)
00196 {
00197     std::vector<uint8_t> response;
00198 
00199     if (read(servo_id, DXL_CW_ANGLE_LIMIT_L, 4, response))
00200     {
00201         checkForErrors(servo_id, response[4], "getAngleLimits");
00202         cw_angle_limit = response[5] + (response[6] << 8);
00203         ccw_angle_limit = response[7] + (response[8] << 8);
00204         return true;
00205     }
00206 
00207     return false;
00208 }
00209 
00210 bool DynamixelIO::getCWAngleLimit(int servo_id, uint16_t& cw_angle)
00211 {
00212     std::vector<uint8_t> response;
00213 
00214     if (read(servo_id, DXL_CW_ANGLE_LIMIT_L, 2, response))
00215     {
00216         checkForErrors(servo_id, response[4], "getCWAngleLimit");
00217         cw_angle = response[5] + (response[6] << 8);
00218         return true;
00219     }
00220 
00221     return false;
00222 }
00223 
00224 bool DynamixelIO::getCCWAngleLimit(int servo_id, uint16_t& ccw_angle)
00225 {
00226     std::vector<uint8_t> response;
00227 
00228     if (read(servo_id, DXL_CCW_ANGLE_LIMIT_L, 2, response))
00229     {
00230         checkForErrors(servo_id, response[4], "getCCWAngleLimit");
00231         ccw_angle = response[5] + (response[6] << 8);
00232         return true;
00233     }
00234 
00235     return false;
00236 }
00237 
00238 bool DynamixelIO::getVoltageLimits(int servo_id, float& min_voltage_limit, float& max_voltage_limit)
00239 {
00240     std::vector<uint8_t> response;
00241 
00242     if (read(servo_id, DXL_DOWN_LIMIT_VOLTAGE, 2, response))
00243     {
00244         checkForErrors(servo_id, response[4], "getVoltageLimits");
00245         min_voltage_limit = response[5] / 10.0;
00246         max_voltage_limit = response[6] / 10.0;
00247         return true;
00248     }
00249 
00250     return false;
00251 }
00252 
00253 bool DynamixelIO::getMinVoltageLimit(int servo_id, float& min_voltage_limit)
00254 {
00255     std::vector<uint8_t> response;
00256 
00257     if (read(servo_id, DXL_DOWN_LIMIT_VOLTAGE, 1, response))
00258     {
00259         checkForErrors(servo_id, response[4], "getMinVoltageLimit");
00260         min_voltage_limit = response[5] / 10.0;
00261         return true;
00262     }
00263 
00264     return false;
00265 }
00266 
00267 bool DynamixelIO::getMaxVoltageLimit(int servo_id, float& max_voltage_limit)
00268 {
00269     std::vector<uint8_t> response;
00270 
00271     if (read(servo_id, DXL_UP_LIMIT_VOLTAGE, 1, response))
00272     {
00273         checkForErrors(servo_id, response[4], "getMaxVoltageLimit");
00274         max_voltage_limit = response[5] / 10.0;
00275         return true;
00276     }
00277 
00278     return false;
00279 }
00280 
00281 bool DynamixelIO::getTemperatureLimit(int servo_id, uint8_t& max_temperature)
00282 {
00283     std::vector<uint8_t> response;
00284 
00285     if (read(servo_id, DXL_LIMIT_TEMPERATURE, 1, response))
00286     {
00287         checkForErrors(servo_id, response[4], "getTemperatureLimit");
00288         max_temperature = response[5];
00289         return true;
00290     }
00291 
00292     return false;
00293 }
00294 
00295 bool DynamixelIO::getMaxTorque(int servo_id, uint16_t& max_torque)
00296 {
00297     std::vector<uint8_t> response;
00298 
00299     if (read(servo_id, DXL_MAX_TORQUE_L, 2, response))
00300     {
00301         checkForErrors(servo_id, response[4], "getMaximumTorque");
00302         max_torque = response[5] + (response[6] << 8);
00303         return true;
00304     }
00305 
00306     return false;
00307 }
00308 
00309 bool DynamixelIO::getAlarmLed(int servo_id, uint8_t& alarm_led)
00310 {
00311     std::vector<uint8_t> response;
00312 
00313     if (read(servo_id, DXL_ALARM_LED, 1, response))
00314     {
00315         checkForErrors(servo_id, response[4], "getAlarmLed");
00316         alarm_led = response[5];
00317         return true;
00318     }
00319 
00320     return false;
00321 }
00322 
00323 bool DynamixelIO::getAlarmShutdown(int servo_id, uint8_t& alarm_shutdown)
00324 {
00325     std::vector<uint8_t> response;
00326 
00327     if (read(servo_id, DXL_ALARM_SHUTDOWN, 1, response))
00328     {
00329         checkForErrors(servo_id, response[4], "getAlarmShutdown");
00330         alarm_shutdown = response[5];
00331         return true;
00332     }
00333 
00334     return false;
00335 }
00336 
00337 
00338 bool DynamixelIO::getTorqueEnable(int servo_id, bool& torque_enabled)
00339 {
00340     std::vector<uint8_t> response;
00341 
00342     if (read(servo_id, DXL_TORQUE_ENABLE, 1, response))
00343     {
00344         checkForErrors(servo_id, response[4], "getTorqueEnable");
00345         torque_enabled = response[5];
00346         return true;
00347     }
00348 
00349     return false;
00350 }
00351 
00352 bool DynamixelIO::getLedStatus(int servo_id, bool& led_enabled)
00353 {
00354     std::vector<uint8_t> response;
00355 
00356     if (read(servo_id, DXL_LED, 1, response))
00357     {
00358         checkForErrors(servo_id, response[4], "getLedStatus");
00359         led_enabled = response[5];
00360         return true;
00361     }
00362 
00363     return false;
00364 }
00365 
00366 bool DynamixelIO::getComplianceMargins(int servo_id, uint8_t& cw_compliance_margin, uint8_t& ccw_compliance_margin)
00367 {
00368     std::vector<uint8_t> response;
00369 
00370     if (read(servo_id, DXL_CW_COMPLIANCE_MARGIN, 2, response))
00371     {
00372         checkForErrors(servo_id, response[4], "getComplianceMargins");
00373         cw_compliance_margin = response[5];
00374         ccw_compliance_margin = response[6];
00375         return true;
00376     }
00377 
00378     return false;
00379 }
00380 
00381 bool DynamixelIO::getCWComplianceMargin(int servo_id, uint8_t& cw_compliance_margin)
00382 {
00383     std::vector<uint8_t> response;
00384 
00385     if (read(servo_id, DXL_CW_COMPLIANCE_MARGIN, 1, response))
00386     {
00387         checkForErrors(servo_id, response[4], "getCWComplianceMargin");
00388         cw_compliance_margin = response[5];
00389         return true;
00390     }
00391 
00392     return false;
00393 }
00394 
00395 bool DynamixelIO::getCCWComplianceMargin(int servo_id, uint8_t& ccw_compliance_margin)
00396 {
00397     std::vector<uint8_t> response;
00398 
00399     if (read(servo_id, DXL_CCW_COMPLIANCE_MARGIN, 1, response))
00400     {
00401         checkForErrors(servo_id, response[4], "getCCWComplianceMargin");
00402         ccw_compliance_margin = response[5];
00403         return true;
00404     }
00405 
00406     return false;
00407 }
00408 
00409 bool DynamixelIO::getComplianceSlopes(int servo_id, uint8_t& cw_compliance_slope, uint8_t& ccw_compliance_slope)
00410 {
00411     std::vector<uint8_t> response;
00412 
00413     if (read(servo_id, DXL_CW_COMPLIANCE_SLOPE, 2, response))
00414     {
00415         checkForErrors(servo_id, response[4], "getComplianceSlopes");
00416         cw_compliance_slope = response[5];
00417         ccw_compliance_slope = response[6];
00418         return true;
00419     }
00420 
00421     return false;
00422 }
00423 
00424 bool DynamixelIO::getCWComplianceSlope(int servo_id, uint8_t& cw_compliance_slope)
00425 {
00426     std::vector<uint8_t> response;
00427 
00428     if (read(servo_id, DXL_CW_COMPLIANCE_SLOPE, 1, response))
00429     {
00430         checkForErrors(servo_id, response[4], "getCWComplianceSlope");
00431         cw_compliance_slope = response[5];
00432         return true;
00433     }
00434 
00435     return false;
00436 }
00437 
00438 bool DynamixelIO::getCCWComplianceSlope(int servo_id, uint8_t& ccw_compliance_slope)
00439 {
00440     std::vector<uint8_t> response;
00441 
00442     if (read(servo_id, DXL_CCW_COMPLIANCE_SLOPE, 1, response))
00443     {
00444         checkForErrors(servo_id, response[4], "getCCWComplianceSlope");
00445         ccw_compliance_slope = response[5];
00446         return true;
00447     }
00448 
00449     return false;
00450 }
00451 
00452 
00453 bool DynamixelIO::getTargetPosition(int servo_id, uint16_t& target_position)
00454 {
00455     std::vector<uint8_t> response;
00456 
00457     if (read(servo_id, DXL_GOAL_POSITION_L, 2, response))
00458     {
00459         checkForErrors(servo_id, response[4], "getTargetPosition");
00460         target_position = response[5] + (response[6] << 8);
00461         return true;
00462     }
00463 
00464     return false;
00465 }
00466 
00467 bool DynamixelIO::getTargetVelocity(int servo_id, int16_t& target_velocity)
00468 {
00469     std::vector<uint8_t> response;
00470 
00471     if (read(servo_id, DXL_GOAL_SPEED_L, 2, response))
00472     {
00473         checkForErrors(servo_id, response[4], "getTargetVelocity");
00474         target_velocity = response[5] + (response[6] << 8);
00475         int direction = (target_velocity & (1 << 10)) == 0 ? 1 : -1;
00476         target_velocity = direction * (target_velocity & DXL_MAX_VELOCITY_ENCODER);
00477         return true;
00478     }
00479 
00480     return false;
00481 }
00482 
00483 bool DynamixelIO::getTorqueLimit(int servo_id, uint16_t& torque_limit)
00484 {
00485     std::vector<uint8_t> response;
00486 
00487     if (read(servo_id, DXL_TORQUE_LIMIT_L, 2, response))
00488     {
00489         checkForErrors(servo_id, response[4], "getTorqueLimit");
00490         torque_limit = response[5] + (response[6] << 8);
00491         return true;
00492     }
00493 
00494     return false;
00495 }
00496 
00497 
00498 bool DynamixelIO::getPosition(int servo_id, uint16_t& position)
00499 {
00500     std::vector<uint8_t> response;
00501 
00502     if (read(servo_id, DXL_PRESENT_POSITION_L, 2, response))
00503     {
00504         checkForErrors(servo_id, response[4], "getPosition");
00505         position = response[5] + (response[6] << 8);
00506         return true;
00507     }
00508 
00509     return false;
00510 }
00511 
00512 bool DynamixelIO::getVelocity(int servo_id, int16_t& velocity)
00513 {
00514     std::vector<uint8_t> response;
00515 
00516     if (read(servo_id, DXL_PRESENT_SPEED_L, 2, response))
00517     {
00518         checkForErrors(servo_id, response[4], "getVelocity");
00519         velocity = response[5] + (response[6] << 8);
00520         int direction = (velocity & (1 << 10)) == 0 ? 1 : -1;
00521         velocity = direction * (velocity & DXL_MAX_VELOCITY_ENCODER);
00522         return true;
00523     }
00524 
00525     return false;
00526 }
00527 
00528 bool DynamixelIO::getLoad(int servo_id, int16_t& load)
00529 {
00530     std::vector<uint8_t> response;
00531 
00532     if (read(servo_id, DXL_PRESENT_LOAD_L, 2, response))
00533     {
00534         checkForErrors(servo_id, response[4], "getLoad");
00535         load = response[5] + (response[6] << 8);
00536         int direction = (load & (1 << 10)) == 0 ? 1 : -1;
00537         load = direction * (load & DXL_MAX_LOAD_ENCODER);
00538         return true;
00539     }
00540 
00541     return false;
00542 }
00543 
00544 bool DynamixelIO::getVoltage(int servo_id, float& voltage)
00545 {
00546     std::vector<uint8_t> response;
00547 
00548     if (read(servo_id, DXL_PRESENT_VOLTAGE, 1, response))
00549     {
00550         checkForErrors(servo_id, response[4], "getVoltage");
00551         voltage = response[5] / 10.0;
00552         return true;
00553     }
00554 
00555     return false;
00556 }
00557 
00558 bool DynamixelIO::getTemperature(int servo_id, uint8_t& temperature)
00559 {
00560     std::vector<uint8_t> response;
00561 
00562     if (read(servo_id, DXL_PRESENT_TEMPERATURE, 1, response))
00563     {
00564         checkForErrors(servo_id, response[4], "getTemperature");
00565         temperature = response[5];
00566         return true;
00567     }
00568 
00569     return false;
00570 }
00571 
00572 bool DynamixelIO::getMoving(int servo_id, bool& is_moving)
00573 {
00574     std::vector<uint8_t> response;
00575 
00576     if (read(servo_id, DXL_LED, 1, response))
00577     {
00578         checkForErrors(servo_id, response[4], "getMoving");
00579         is_moving = response[5];
00580         return true;
00581     }
00582 
00583     return false;
00584 }
00585 
00586 bool DynamixelIO::getFeedback(int servo_id, DynamixelStatus& status)
00587 {
00588     std::vector<uint8_t> response;
00589 
00590     if (read(servo_id, DXL_TORQUE_LIMIT_L, 13, response))
00591     {
00592         struct timespec ts_now;
00593         clock_gettime(CLOCK_REALTIME, &ts_now);
00594         double timestamp = ts_now.tv_sec + ts_now.tv_nsec / 1.0e9;
00595 
00596         checkForErrors(servo_id, response[4], "getFeedback");
00597 
00598         if (response.size() == 19)
00599         {
00600             int offset = 5;
00601             
00602             uint16_t torque_limit = response[offset+0] + (response[offset+1] << 8);
00603             uint16_t position = response[offset+2] + (response[offset+3] << 8);
00604 
00605             int16_t velocity = response[offset+4] + (response[offset+5] << 8);
00606             int direction = (velocity & (1 << 10)) == 0 ? 1 : -1;
00607             velocity = direction * (velocity & DXL_MAX_VELOCITY_ENCODER);
00608 
00609             int16_t load = response[offset+6] + (response[offset+7] << 8);
00610             direction = (load & (1 << 10)) == 0 ? 1 : -1;
00611             load = direction * (load & DXL_MAX_LOAD_ENCODER);
00612 
00613             uint8_t voltage = response[offset+8];
00614             uint8_t temperature = response[offset+9];
00615             bool moving = response[offset+12];
00616 
00617             status.timestamp = timestamp;
00618             status.torque_limit = torque_limit;
00619             status.position = position;
00620             status.velocity = velocity;
00621             status.load = load;
00622             status.voltage = voltage;
00623             status.temperature = temperature;
00624             status.moving = moving;
00625 
00626             return true;
00627         }
00628     }
00629 
00630     return false;
00631 }
00632 
00633 
00634 
00635 /************************ SETTERS **************************/
00636 
00637 bool DynamixelIO::setId(int servo_id, uint8_t id)
00638 {
00639     std::vector<uint8_t> data;
00640     data.push_back(id);
00641     
00642     std::vector<uint8_t> response;
00643     
00644     if (write(servo_id, DXL_ID, data, response))
00645     {
00646         checkForErrors(servo_id, response[4], "setId");
00647         return true;
00648     }
00649     
00650     return false;
00651 }
00652 
00653 bool DynamixelIO::setBaudRate(int servo_id, uint8_t baud_rate)
00654 {
00655     std::vector<uint8_t> data;
00656     data.push_back(baud_rate);
00657     
00658     std::vector<uint8_t> response;
00659     
00660     if (write(servo_id, DXL_BAUD_RATE, data, response))
00661     {
00662         DynamixelData* dd = findCachedParameters(servo_id);
00663         dd->baud_rate = baud_rate;
00664         
00665         checkForErrors(servo_id, response[4], "setBaudRate");
00666         return true;
00667     }
00668     
00669     return false;
00670 }
00671 
00672 bool DynamixelIO::setReturnDelayTime(int servo_id, uint8_t return_delay_time)
00673 {
00674     std::vector<uint8_t> data;
00675     data.push_back(return_delay_time);
00676 
00677     std::vector<uint8_t> response;
00678 
00679     if (write(servo_id, DXL_RETURN_DELAY_TIME, data, response))
00680     {
00681         DynamixelData* dd = findCachedParameters(servo_id);
00682         dd->return_delay_time = return_delay_time;
00683         
00684         checkForErrors(servo_id, response[4], "setReturnDelayTime");
00685         return true;
00686     }
00687 
00688     return false;
00689 }
00690 
00691 bool DynamixelIO::setAngleLimits(int servo_id, uint16_t cw_angle, uint16_t ccw_angle)
00692 {
00693     std::vector<uint8_t> data;
00694     data.push_back(cw_angle % 256);     // lo_byte
00695     data.push_back(cw_angle >> 8);      // hi_byte
00696     data.push_back(ccw_angle % 256);    // lo_byte
00697     data.push_back(ccw_angle >> 8);     // hi_byte
00698     
00699     std::vector<uint8_t> response;
00700     
00701     if (write(servo_id, DXL_CW_ANGLE_LIMIT_L, data, response))
00702     {
00703         DynamixelData* dd = findCachedParameters(servo_id);
00704         dd->cw_angle_limit = cw_angle;
00705         dd->ccw_angle_limit = ccw_angle;
00706         
00707         checkForErrors(servo_id, response[4], "setAngleLimits");
00708         return true;
00709     }
00710     
00711     return false;
00712 }
00713 
00714 bool DynamixelIO::setCWAngleLimit(int servo_id, uint16_t cw_angle)
00715 {
00716     std::vector<uint8_t> data;
00717     data.push_back(cw_angle % 256); // lo_byte
00718     data.push_back(cw_angle >> 8);  // hi_byte
00719     
00720     std::vector<uint8_t> response;
00721     
00722     if (write(servo_id, DXL_CW_ANGLE_LIMIT_L, data, response))
00723     {
00724         DynamixelData* dd = findCachedParameters(servo_id);
00725         dd->cw_angle_limit = cw_angle;
00726         
00727         checkForErrors(servo_id, response[4], "setCWAngleLimit");
00728         return true;
00729     }
00730     
00731     return false;
00732 }
00733 
00734 bool DynamixelIO::setCCWAngleLimit(int servo_id, uint16_t ccw_angle)
00735 {
00736     std::vector<uint8_t> data;
00737     data.push_back(ccw_angle % 256); // lo_byte
00738     data.push_back(ccw_angle >> 8);  // hi_byte
00739     
00740     std::vector<uint8_t> response;
00741     
00742     if (write(servo_id, DXL_CCW_ANGLE_LIMIT_L, data, response))
00743     {
00744         DynamixelData* dd = findCachedParameters(servo_id);
00745         dd->ccw_angle_limit = ccw_angle;
00746         
00747         checkForErrors(servo_id, response[4], "setCCWAngleLimit");
00748         return true;
00749     }
00750     
00751     return false;
00752 }
00753 
00754 bool DynamixelIO::setVoltageLimits(int servo_id, float min_voltage_limit, float max_voltage_limit)
00755 {
00756     uint8_t min_voltage = min_voltage_limit * 10;
00757     uint8_t max_voltage = max_voltage_limit * 10;
00758     
00759     std::vector<uint8_t> data;
00760     data.push_back(min_voltage);
00761     data.push_back(max_voltage);
00762     
00763     std::vector<uint8_t> response;
00764     
00765     if (write(servo_id, DXL_DOWN_LIMIT_VOLTAGE, data, response))
00766     {
00767         DynamixelData* dd = findCachedParameters(servo_id);
00768         dd->voltage_limit_low = min_voltage;
00769         dd->voltage_limit_high = max_voltage;
00770         
00771         checkForErrors(servo_id, response[4], "setVoltageLimits");
00772         return true;
00773     }
00774     
00775     return false;
00776 }
00777 
00778 bool DynamixelIO::setMinVoltageLimit(int servo_id, float min_voltage_limit)
00779 {
00780     uint8_t min_voltage = min_voltage_limit * 10;
00781     
00782     std::vector<uint8_t> data;
00783     data.push_back(min_voltage);
00784     
00785     std::vector<uint8_t> response;
00786     
00787     if (write(servo_id, DXL_DOWN_LIMIT_VOLTAGE, data, response))
00788     {
00789         DynamixelData* dd = findCachedParameters(servo_id);
00790         dd->voltage_limit_low = min_voltage;
00791         
00792         checkForErrors(servo_id, response[4], "setMinVoltageLimit");
00793         return true;
00794     }
00795     
00796     return false;
00797 }
00798 
00799 bool DynamixelIO::setMaxVoltageLimit(int servo_id, float max_voltage_limit)
00800 {
00801     uint8_t max_voltage = max_voltage_limit * 10;
00802     
00803     std::vector<uint8_t> data;
00804     data.push_back(max_voltage);
00805     
00806     std::vector<uint8_t> response;
00807     
00808     if (write(servo_id, DXL_UP_LIMIT_VOLTAGE, data, response))
00809     {
00810         DynamixelData* dd = findCachedParameters(servo_id);
00811         dd->voltage_limit_high = max_voltage;
00812         
00813         checkForErrors(servo_id, response[4], "setMaxVoltageLimit");
00814         return true;
00815     }
00816     
00817     return false;
00818 }
00819 
00820 bool DynamixelIO::setTemperatureLimit(int servo_id, uint8_t max_temperature)
00821 {
00822     std::vector<uint8_t> data;
00823     data.push_back(max_temperature);
00824 
00825     std::vector<uint8_t> response;
00826 
00827     if (write(servo_id, DXL_LIMIT_TEMPERATURE, data, response))
00828     {
00829         DynamixelData* dd = findCachedParameters(servo_id);
00830         dd->temperature_limit = max_temperature;
00831         
00832         checkForErrors(servo_id, response[4], "setTemperatureLimit");
00833         return true;
00834     }
00835 
00836     return false;
00837 }
00838 
00839 bool DynamixelIO::setMaxTorque(int servo_id, uint16_t max_torque)
00840 {
00841     std::vector<uint8_t> data;
00842     data.push_back(max_torque % 256); // lo_byte
00843     data.push_back(max_torque >> 8);  // hi_byte
00844     
00845     std::vector<uint8_t> response;
00846     
00847     if (write(servo_id, DXL_MAX_TORQUE_L, data, response))
00848     {
00849         DynamixelData* dd = findCachedParameters(servo_id);
00850         dd->max_torque = max_torque;
00851         
00852         checkForErrors(servo_id, response[4], "setMaxTorque");
00853         return true;
00854     }
00855     
00856     return false;
00857 }
00858 
00859 bool DynamixelIO::setAlarmLed(int servo_id, uint8_t alarm_led)
00860 {
00861     std::vector<uint8_t> data;
00862     data.push_back(alarm_led);
00863 
00864     std::vector<uint8_t> response;
00865 
00866     if (write(servo_id, DXL_ALARM_LED, data, response))
00867     {
00868         DynamixelData* dd = findCachedParameters(servo_id);
00869         dd->alarm_led = alarm_led;
00870         
00871         checkForErrors(servo_id, response[4], "setAlarmLed");
00872         return true;
00873     }
00874 
00875     return false;
00876 }
00877 
00878 bool DynamixelIO::setAlarmShutdown(int servo_id, uint8_t alarm_shutdown)
00879 {
00880     std::vector<uint8_t> data;
00881     data.push_back(alarm_shutdown);
00882 
00883     std::vector<uint8_t> response;
00884 
00885     if (write(servo_id, DXL_ALARM_SHUTDOWN, data, response))
00886     {
00887         DynamixelData* dd = findCachedParameters(servo_id);
00888         dd->alarm_shutdown = alarm_shutdown;
00889         
00890         checkForErrors(servo_id, response[4], "setAlarmShutdown");
00891         return true;
00892     }
00893 
00894     return false;
00895 }
00896 
00897 bool DynamixelIO::setTorqueEnable(int servo_id, bool on)
00898 {
00899     std::vector<uint8_t> data;
00900     data.push_back(on);
00901 
00902     std::vector<uint8_t> response;
00903 
00904     if (write(servo_id, DXL_TORQUE_ENABLE, data, response))
00905     {
00906         DynamixelData* dd = findCachedParameters(servo_id);
00907         dd->torque_enabled = on;
00908         
00909         checkForErrors(servo_id, response[4], "setTorqueEnable");
00910         return true;
00911     }
00912 
00913     return false;
00914 }
00915 
00916 bool DynamixelIO::setLed(int servo_id, bool on)
00917 {
00918     std::vector<uint8_t> data;
00919     data.push_back(on);
00920 
00921     std::vector<uint8_t> response;
00922 
00923     if (write(servo_id, DXL_LED, data, response))
00924     {
00925         DynamixelData* dd = findCachedParameters(servo_id);
00926         dd->led = on;
00927         
00928         checkForErrors(servo_id, response[4], "setLed");
00929         return true;
00930     }
00931 
00932     return false;
00933 }
00934 
00935 bool DynamixelIO::setComplianceMargins(int servo_id, uint8_t cw_margin, uint8_t ccw_margin)
00936 {
00937     std::vector<uint8_t> data;
00938     data.push_back(cw_margin);
00939     data.push_back(ccw_margin);
00940 
00941     std::vector<uint8_t> response;
00942 
00943     if (write(servo_id, DXL_CW_COMPLIANCE_MARGIN, data, response))
00944     {
00945         DynamixelData* dd = findCachedParameters(servo_id);
00946         dd->cw_compliance_margin = cw_margin;
00947         dd->ccw_compliance_margin = ccw_margin;
00948         
00949         checkForErrors(servo_id, response[4], "setComplianceMargins");
00950         return true;
00951     }
00952 
00953     return false;
00954 }
00955 
00956 bool DynamixelIO::setCWComplianceMargin(int servo_id, uint8_t cw_margin)
00957 {
00958     std::vector<uint8_t> data;
00959     data.push_back(cw_margin);
00960 
00961     std::vector<uint8_t> response;
00962 
00963     if (write(servo_id, DXL_CW_COMPLIANCE_MARGIN, data, response))
00964     {
00965         DynamixelData* dd = findCachedParameters(servo_id);
00966         dd->cw_compliance_margin = cw_margin;
00967         
00968         checkForErrors(servo_id, response[4], "setCWComplianceMargin");
00969         return true;
00970     }
00971 
00972     return false;
00973 }
00974 
00975 bool DynamixelIO::setCCWComplianceMargin(int servo_id, uint8_t ccw_margin)
00976 {
00977     std::vector<uint8_t> data;
00978     data.push_back(ccw_margin);
00979 
00980     std::vector<uint8_t> response;
00981 
00982     if (write(servo_id, DXL_CCW_COMPLIANCE_MARGIN, data, response))
00983     {
00984         DynamixelData* dd = findCachedParameters(servo_id);
00985         dd->ccw_compliance_margin = ccw_margin;
00986         
00987         checkForErrors(servo_id, response[4], "setCCWComplianceMargin");
00988         return true;
00989     }
00990 
00991     return false;
00992 }
00993 
00994 bool DynamixelIO::setComplianceSlopes(int servo_id, uint8_t cw_slope, uint8_t ccw_slope)
00995 {
00996     std::vector<uint8_t> data;
00997     data.push_back(cw_slope);
00998     data.push_back(ccw_slope);
00999 
01000     std::vector<uint8_t> response;
01001 
01002     if (write(servo_id, DXL_CW_COMPLIANCE_SLOPE, data, response))
01003     {
01004         DynamixelData* dd = findCachedParameters(servo_id);
01005         dd->cw_compliance_slope = cw_slope;
01006         dd->ccw_compliance_slope = ccw_slope;
01007         
01008         checkForErrors(servo_id, response[4], "setComplianceSlopes");
01009         return true;
01010     }
01011 
01012     return false;
01013 }
01014 
01015 bool DynamixelIO::setCWComplianceSlope(int servo_id, uint8_t cw_slope)
01016 {
01017     std::vector<uint8_t> data;
01018     data.push_back(cw_slope);
01019 
01020     std::vector<uint8_t> response;
01021 
01022     if (write(servo_id, DXL_CW_COMPLIANCE_SLOPE, data, response))
01023     {
01024         DynamixelData* dd = findCachedParameters(servo_id);
01025         dd->cw_compliance_slope = cw_slope;
01026         
01027         checkForErrors(servo_id, response[4], "setCWComplianceSlope");
01028         return true;
01029     }
01030 
01031     return false;
01032 }
01033 
01034 bool DynamixelIO::setCCWComplianceSlope(int servo_id, uint8_t ccw_slope)
01035 {
01036     std::vector<uint8_t> data;
01037     data.push_back(ccw_slope);
01038 
01039     std::vector<uint8_t> response;
01040 
01041     if (write(servo_id, DXL_CCW_COMPLIANCE_SLOPE, data, response))
01042     {
01043         DynamixelData* dd = findCachedParameters(servo_id);
01044         dd->ccw_compliance_slope = ccw_slope;
01045         
01046         checkForErrors(servo_id, response[4], "setCCWComplianceSlope");
01047         return true;
01048     }
01049 
01050     return false;
01051 }
01052 
01053 bool DynamixelIO::setPosition(int servo_id, uint16_t position)
01054 {
01055     std::vector<uint8_t> data;
01056     data.push_back(position % 256); // lo_byte
01057     data.push_back(position >> 8);  // hi_byte
01058 
01059     std::vector<uint8_t> response;
01060 
01061     if (write(servo_id, DXL_GOAL_POSITION_L, data, response))
01062     {
01063         DynamixelData* dd = findCachedParameters(servo_id);
01064         dd->target_position = position;
01065         dd->torque_enabled = true;
01066         
01067         checkForErrors(servo_id, response[4], "setPosition");
01068         return true;
01069     }
01070 
01071     return false;
01072 }
01073 
01074 
01075 bool DynamixelIO::setVelocity(int servo_id, int16_t velocity)
01076 {
01077     std::vector<uint8_t> data;
01078 
01079     if (velocity >= 0)
01080     {
01081         data.push_back(velocity % 256); // lo_byte
01082         data.push_back(velocity >> 8);  // hi_byte
01083     }
01084     else
01085     {
01086         data.push_back((DXL_MAX_VELOCITY_ENCODER - velocity) % 256); // lo_byte
01087         data.push_back((DXL_MAX_VELOCITY_ENCODER - velocity) >> 8);  // hi_byte
01088     }
01089 
01090     std::vector<uint8_t> response;
01091 
01092     if (write(servo_id, DXL_GOAL_SPEED_L, data, response))
01093     {
01094         DynamixelData* dd = findCachedParameters(servo_id);
01095         dd->target_velocity = velocity;
01096         dd->torque_enabled = true;
01097 
01098         checkForErrors(servo_id, response[4], "setVelocity");
01099         return true;
01100     }
01101 
01102     return false;
01103 }
01104 
01105 bool DynamixelIO::setTorqueLimit(int servo_id, uint16_t torque_limit)
01106 {
01107     std::vector<uint8_t> data;
01108     data.push_back(torque_limit % 256); // lo_byte
01109     data.push_back(torque_limit >> 8);  // hi_byte
01110 
01111     std::vector<uint8_t> response;
01112 
01113     if (write(servo_id, DXL_TORQUE_LIMIT_L, data, response))
01114     {
01115         checkForErrors(servo_id, response[4], "setTorqueLimit");
01116         return true;
01117     }
01118 
01119     return false;
01120 }
01121 
01122 
01123 bool DynamixelIO::setMultiPosition(std::vector<std::vector<int> > value_pairs)
01124 {
01125     std::vector<std::vector<uint8_t> > data;
01126 
01127     for (size_t i = 0; i < value_pairs.size(); ++i)
01128     {
01129         int motor_id = value_pairs[i][0];
01130         int position = value_pairs[i][1];
01131 
01132         DynamixelData* dd = findCachedParameters(motor_id);
01133         dd->target_position = position;
01134         dd->torque_enabled = true;
01135         
01136         std::vector<uint8_t> value_pair;
01137         value_pair.push_back(motor_id);                 // servo id
01138         value_pair.push_back(position % 256);           // lo_byte
01139         value_pair.push_back(position >> 8);            // hi_byte
01140 
01141         data.push_back(value_pair);
01142     }
01143 
01144     if (syncWrite(DXL_GOAL_POSITION_L, data)) { return true; }
01145     else { return false; }
01146 }
01147 
01148 bool DynamixelIO::setMultiVelocity(std::vector<std::vector<int> > value_pairs)
01149 {
01150     std::vector<std::vector<uint8_t> > data;
01151 
01152     for (size_t i = 0; i < value_pairs.size(); ++i)
01153     {
01154         int motor_id = value_pairs[i][0];
01155         int velocity = value_pairs[i][1];
01156 
01157         DynamixelData* dd = findCachedParameters(motor_id);
01158         dd->target_velocity = velocity;
01159         dd->torque_enabled = true;
01160         
01161         std::vector<uint8_t> value_pair;
01162         value_pair.push_back(motor_id);             // servo id
01163 
01164         if (velocity >= 0)
01165         {
01166             value_pair.push_back(velocity % 256);   // lo_byte
01167             value_pair.push_back(velocity >> 8);    // hi_byte
01168         }
01169         else
01170         {
01171             value_pair.push_back((DXL_MAX_VELOCITY_ENCODER - velocity) % 256);  // lo_byte
01172             value_pair.push_back((DXL_MAX_VELOCITY_ENCODER - velocity) >> 8);   // hi_byte
01173         }
01174 
01175         data.push_back(value_pair);
01176     }
01177 
01178     if (syncWrite(DXL_GOAL_SPEED_L, data)) { return true; }
01179     else { return false; }
01180 }
01181 
01182 bool DynamixelIO::setMultiPositionVelocity(std::vector<std::vector<int> > value_tuples)
01183 {
01184     std::vector<std::vector<uint8_t> > data;
01185 
01186     for (size_t i = 0; i < value_tuples.size(); ++i)
01187     {
01188         int motor_id = value_tuples[i][0];
01189         int position = value_tuples[i][1];
01190         int velocity = value_tuples[i][2];
01191 
01192         DynamixelData* dd = findCachedParameters(motor_id);
01193         dd->target_position = position;
01194         dd->target_velocity = velocity;
01195         dd->torque_enabled = true;
01196 
01197         std::vector<uint8_t> vals;
01198 
01199         vals.push_back(motor_id);     // servo id
01200         vals.push_back(position % 256);         // lo_byte
01201         vals.push_back(position >> 8);          // hi_byte
01202 
01203         if (velocity >= 0)
01204         {
01205             vals.push_back(velocity % 256);     // lo_byte
01206             vals.push_back(velocity >> 8);      // hi_byte
01207         }
01208         else
01209         {
01210             vals.push_back((DXL_MAX_VELOCITY_ENCODER - velocity) % 256);    // lo_byte
01211             vals.push_back((DXL_MAX_VELOCITY_ENCODER - velocity) >> 8);     // hi_byte
01212         }
01213 
01214         data.push_back(vals);
01215     }
01216 
01217     if (syncWrite(DXL_GOAL_POSITION_L, data)) { return true; }
01218     else { return false; }
01219 }
01220 
01221 bool DynamixelIO::setMultiComplianceMargins(std::vector<std::vector<int> > value_pairs)
01222 {
01223     std::vector<std::vector<uint8_t> > data;
01224     
01225     for (size_t i = 0; i < value_pairs.size(); ++i)
01226     {
01227         int motor_id = value_pairs[i][0];
01228         int cw_margin = value_pairs[i][1];
01229         int ccw_margin = value_pairs[i][2];
01230         
01231         DynamixelData* dd = findCachedParameters(motor_id);
01232         dd->cw_compliance_margin = cw_margin;
01233         dd->ccw_compliance_margin = ccw_margin;
01234         
01235         std::vector<uint8_t> value_pair;
01236         value_pair.push_back(motor_id);         // servo id
01237         value_pair.push_back(cw_margin);        // cw_compliance_margin
01238         value_pair.push_back(ccw_margin);       // ccw_compliance_margin
01239         
01240         data.push_back(value_pair);
01241     }
01242     
01243     if (syncWrite(DXL_CW_COMPLIANCE_MARGIN, data)) { return true; }
01244     else { return false; }
01245 }
01246 
01247 bool DynamixelIO::setMultiComplianceSlopes(std::vector<std::vector<int> > value_pairs)
01248 {
01249     std::vector<std::vector<uint8_t> > data;
01250     
01251     for (size_t i = 0; i < value_pairs.size(); ++i)
01252     {
01253         int motor_id = value_pairs[i][0];
01254         int cw_slope = value_pairs[i][1];
01255         int ccw_slope = value_pairs[i][2];
01256         
01257         DynamixelData* dd = findCachedParameters(motor_id);
01258         dd->cw_compliance_slope = cw_slope;
01259         dd->ccw_compliance_slope = ccw_slope;
01260         
01261         std::vector<uint8_t> value_pair;
01262         value_pair.push_back(motor_id);     // servo id
01263         value_pair.push_back(cw_slope);     // cw_compliance_slope
01264         value_pair.push_back(ccw_slope);    // ccw_compliance_slope
01265         
01266         data.push_back(value_pair);
01267     }
01268     
01269     if (syncWrite(DXL_CW_COMPLIANCE_SLOPE, data)) { return true; }
01270     else { return false; }
01271 }
01272 
01273 bool DynamixelIO::setMultiTorqueEnabled(std::vector<std::vector<int> > value_pairs)
01274 {
01275     std::vector<std::vector<uint8_t> > data;
01276     
01277     for (size_t i = 0; i < value_pairs.size(); ++i)
01278     {
01279         int motor_id = value_pairs[i][0];
01280         bool torque_enabled = value_pairs[i][1];
01281         
01282         DynamixelData* dd = findCachedParameters(motor_id);
01283         dd->torque_enabled = torque_enabled;
01284         
01285         std::vector<uint8_t> value_pair;
01286         value_pair.push_back(motor_id);         // servo id
01287         value_pair.push_back(torque_enabled);   // torque_enabled
01288         
01289         data.push_back(value_pair);
01290     }
01291     
01292     if (syncWrite(DXL_TORQUE_ENABLE, data)) { return true; }
01293     else { return false; }
01294 }
01295 
01296 bool DynamixelIO::setMultiTorqueLimit(std::vector<std::vector<int> > value_pairs)
01297 {
01298     std::vector<std::vector<uint8_t> > data;
01299 
01300     for (size_t i = 0; i < value_pairs.size(); ++i)
01301     {
01302         std::vector<uint8_t> value_pair;
01303 
01304         int torque_limit = value_pairs[i][1];
01305 
01306         value_pair.push_back(value_pairs[i][0]);        // servo id
01307         value_pair.push_back(torque_limit % 256);       // lo_byte
01308         value_pair.push_back(torque_limit >> 8);        // hi_byte
01309 
01310         data.push_back(value_pair);
01311     }
01312 
01313     if (syncWrite(DXL_TORQUE_LIMIT_L, data)) { return true; }
01314     else { return false; }
01315 }
01316 
01317 bool DynamixelIO::setMultiValues(std::vector<std::map<std::string, int> > value_maps)
01318 {
01319     std::vector<std::vector<uint8_t> > data;
01320     
01321     for (size_t i = 0; i < value_maps.size(); ++i)
01322     {
01323         std::map<std::string, int> m = value_maps[i];
01324         std::map<std::string, int>::const_iterator it;
01325         
01326         it = m.find("id");
01327         if (it == m.end()) { return false; }
01328         
01329         int id = it->second;
01330         const DynamixelData* cache = getCachedParameters(id);
01331         if (cache == NULL) { return false; }
01332         
01333         bool     torque_enabled = cache->torque_enabled;
01334         uint8_t  led = cache->led;
01335         uint8_t  cw_compliance_margin = cache->cw_compliance_margin;
01336         uint8_t  ccw_compliance_margin = cache->ccw_compliance_margin;
01337         uint8_t  cw_compliance_slope = cache->cw_compliance_slope;
01338         uint8_t  ccw_compliance_slope = cache->ccw_compliance_slope;
01339         uint16_t target_position = cache->target_position;
01340         int16_t  target_velocity = cache->target_velocity;
01341         
01342         it = m.find("torque_enabled");
01343         if (it != m.end()) { torque_enabled = it->second; }
01344         
01345         it = m.find("cw_compliance_margin");
01346         if (it != m.end()) { cw_compliance_margin = it->second; }
01347         
01348         it = m.find("ccw_compliance_margin");
01349         if (it != m.end()) { ccw_compliance_margin = it->second; }
01350         
01351         it = m.find("cw_compliance_slope");
01352         if (it != m.end()) { cw_compliance_slope = it->second; }
01353         
01354         it = m.find("ccw_compliance_slope");
01355         if (it != m.end()) { ccw_compliance_slope = it->second; }
01356         
01357         it = m.find("target_position");
01358         if (it != m.end()) { target_position = it->second; }
01359         
01360         it = m.find("target_velocity");
01361         if (it != m.end()) { target_velocity = it->second; }
01362         
01363         std::vector<uint8_t> vals;
01364         
01365         vals.push_back(id);
01366         
01367         vals.push_back(torque_enabled);
01368         vals.push_back(led);
01369         vals.push_back(cw_compliance_margin);
01370         vals.push_back(ccw_compliance_margin);
01371         vals.push_back(cw_compliance_slope);
01372         vals.push_back(ccw_compliance_slope);
01373         
01374         vals.push_back(target_position % 256);         // lo_byte
01375         vals.push_back(target_position >> 8);          // hi_byte
01376         
01377         if (target_velocity >= 0)
01378         {
01379             vals.push_back(target_velocity % 256);     // lo_byte
01380             vals.push_back(target_velocity >> 8);      // hi_byte
01381         }
01382         else
01383         {
01384             vals.push_back((DXL_MAX_VELOCITY_ENCODER - target_velocity) % 256);    // lo_byte
01385             vals.push_back((DXL_MAX_VELOCITY_ENCODER - target_velocity) >> 8);     // hi_byte
01386         }
01387         
01388         data.push_back(vals);
01389     }
01390 
01391     if (syncWrite(DXL_TORQUE_ENABLE, data)) { return true; }
01392     else { return false; }
01393 }
01394 
01395 bool DynamixelIO::updateCachedParameters(int servo_id, DynamixelData* data)
01396 {
01397     std::vector<uint8_t> response;
01398     if (read(servo_id, DXL_MODEL_NUMBER_L, 34, response))
01399     {
01400         uint8_t byte_num = 5;
01401         
01402         data->model_number = response[byte_num+0] + (response[byte_num+1] << 8);
01403         data->firmware_version = response[byte_num+2];
01404         data->id = response[byte_num+3];
01405         data->baud_rate = response[byte_num+4];
01406         data->return_delay_time = response[byte_num+5];
01407         data->cw_angle_limit = response[byte_num+6] + (response[byte_num+7] << 8);
01408         data->ccw_angle_limit = response[byte_num+8] + (response[byte_num+9] << 8);
01409         data->drive_mode = response[byte_num+10];
01410         data->temperature_limit = response[byte_num+11];
01411         data->voltage_limit_low = response[byte_num+12];
01412         data->voltage_limit_high = response[byte_num+13];
01413         data->max_torque = response[byte_num+14] + (response[byte_num+15] << 8);
01414         data->return_level = response[byte_num+16];
01415         data->alarm_led = response[byte_num+17];
01416         data->alarm_shutdown = response[byte_num+18];
01417         data->torque_enabled = response[byte_num+24];
01418         data->led = response[byte_num+25];
01419         data->cw_compliance_margin = response[byte_num+26];
01420         data->ccw_compliance_margin = response[byte_num+27];
01421         data->cw_compliance_slope = response[byte_num+28];
01422         data->ccw_compliance_slope = response[byte_num+29];
01423         data->target_position = response[byte_num+30] + (response[byte_num+31] << 8);
01424         
01425         int16_t target_velocity = response[byte_num+32] + (response[byte_num+33] << 8);
01426         int direction = (target_velocity & (1 << 10)) == 0 ? 1 : -1;
01427         target_velocity = direction * (target_velocity & DXL_MAX_VELOCITY_ENCODER);
01428         data->target_velocity = target_velocity;
01429         
01430         return true;
01431     }
01432 
01433     return false;
01434 }
01435 
01436 void DynamixelIO::checkForErrors(int servo_id, uint8_t error_code, std::string command_failed)
01437 {
01438     DynamixelData* dd = findCachedParameters(servo_id);
01439     
01440     if (error_code == DXL_NO_ERROR)
01441     {
01442         dd->shutdown_error_time = 0.0;
01443         return;        
01444     }
01445     
01446     std::vector<std::string> error_msgs;
01447 
01448     if ((error_code & DXL_OVERHEATING_ERROR) != 0)
01449     {
01450         if (dd->shutdown_error_time <= 0.0)
01451         {
01452             struct timespec ts_now;
01453             clock_gettime(CLOCK_REALTIME, &ts_now);
01454             dd->shutdown_error_time = ts_now.tv_sec + ts_now.tv_nsec / 1.0e9;
01455         }
01456         
01457         error_msgs.push_back("Overheating Error");
01458     }
01459     
01460     if ((error_code & DXL_OVERLOAD_ERROR) != 0)
01461     {
01462         if (dd->shutdown_error_time <= 0.0)
01463         {
01464             struct timespec ts_now;
01465             clock_gettime(CLOCK_REALTIME, &ts_now);
01466             dd->shutdown_error_time = ts_now.tv_sec + ts_now.tv_nsec / 1.0e9;
01467         }
01468         
01469         error_msgs.push_back("Overload Error");
01470     }
01471     
01472     if ((error_code & DXL_INPUT_VOLTAGE_ERROR) != 0) { error_msgs.push_back("Input Voltage Error"); }
01473     if ((error_code & DXL_ANGLE_LIMIT_ERROR) != 0)   { error_msgs.push_back("Angle Limit Error"); }
01474     if ((error_code & DXL_RANGE_ERROR) != 0)         { error_msgs.push_back("Range Error"); }
01475     if ((error_code & DXL_CHECKSUM_ERROR) != 0)      { error_msgs.push_back("Checksum Error"); }
01476     if ((error_code & DXL_INSTRUCTION_ERROR) != 0)   { error_msgs.push_back("Instruction Error"); }
01477     
01478     std::stringstream m;
01479     m << "Detected error condition [";
01480 
01481     for (size_t i = 0; i < error_msgs.size(); ++i)
01482     {
01483         m << error_msgs[i] << (i != error_msgs.size()-1 ? ", " : "");
01484     }
01485     
01486     m << "] during " << command_failed << " command on servo #" << servo_id; 
01487     dd->error = m.str();
01488     updateCachedParameters(servo_id, dd);
01489 }
01490 
01491 bool DynamixelIO::read(int servo_id,
01492                        int address,
01493                        int size,
01494                        std::vector<uint8_t>& response)
01495 {
01496     // Number of bytes following standard header (0xFF, 0xFF, id, length)
01497     uint8_t length = 4;
01498 
01499     // Check Sum = ~ (ID + LENGTH + INSTRUCTION + PARAM_1 + ... + PARAM_N)
01500     // If the calculated value is > 255, the lower byte is the check sum.
01501     uint8_t checksum = 0xFF - ( (servo_id + length + DXL_READ_DATA + address + size) % 256 );
01502 
01503     // packet: FF  FF  ID LENGTH INSTRUCTION PARAM_1 ... CHECKSUM
01504     uint8_t packet[8] = { 0xFF, 0xFF, servo_id, length, DXL_READ_DATA, address, size, checksum };
01505 
01506     pthread_mutex_lock(&serial_mutex_);
01507     bool success = writePacket(packet, 8);
01508     if (success) { success = readResponse(response); }
01509     pthread_mutex_unlock(&serial_mutex_);
01510 
01511     return success;
01512 }
01513 
01514 bool DynamixelIO::write(int servo_id,
01515                         int address,
01516                         const std::vector<uint8_t>& data,
01517                         std::vector<uint8_t>& response)
01518 {
01519     // Number of bytes following standard header (0xFF, 0xFF, id, length)
01520     // instruction, address, len(data), checksum
01521     uint8_t length = 3 + data.size();
01522 
01523     // Check Sum = ~ (ID + LENGTH + INSTRUCTION + PARAM_1 + ... + PARAM_N)
01524     // If the calculated value is > 255, the lower byte is the check sum.
01525     uint32_t sum = 0;
01526 
01527     for (uint32_t i = 0; i < data.size(); ++i)
01528     {
01529         sum += data[i];
01530     }
01531 
01532     uint8_t checksum = 0xFF - ( (servo_id + length + DXL_WRITE_DATA + address + sum) % 256 );
01533 
01534     // packet: FF  FF  ID LENGTH INSTRUCTION PARAM_1 ... CHECKSUM
01535     uint8_t packetLength = 4 + length;
01536     uint8_t packet[packetLength];
01537 
01538     packet[0] = 0xFF;
01539     packet[1] = 0xFF;
01540     packet[2] = servo_id;
01541     packet[3] = length;
01542     packet[4] = DXL_WRITE_DATA;
01543     packet[5] = address;
01544 
01545     for (uint8_t i = 0; i < data.size(); ++i)
01546     {
01547         packet[6+i] = data[i];
01548     }
01549 
01550     packet[packetLength-1] = checksum;
01551 
01552     pthread_mutex_lock(&serial_mutex_);
01553     bool success = writePacket(packet, packetLength);
01554     if (success) { success = readResponse(response); }
01555     pthread_mutex_unlock(&serial_mutex_);
01556 
01557     return success;
01558 }
01559 
01560 bool DynamixelIO::syncWrite(int address,
01561                             const std::vector<std::vector<uint8_t> >& data)
01562 {
01563     // data = ( (id, byte1, byte2... ), (id, byte1, byte2...), ... )
01564 
01565     // Instruction, address, length, checksum
01566     uint8_t length = 4;
01567 
01568     // Check Sum = ~ (ID + LENGTH + INSTRUCTION + PARAM_1 + ... + PARAM_N)
01569     // If the calculated value is > 255, the lower byte is the check sum.
01570     uint32_t sum = 0;
01571     uint8_t servo_length = 0;
01572 
01573     for (size_t i = 0; i < data.size(); ++i)
01574     {
01575         for (size_t j = 0; j < data[i].size(); ++j)
01576         {
01577             sum += data[i][j];
01578         }
01579 
01580         length += data[i].size();
01581         servo_length = data[i].size() - 1;
01582     }
01583 
01584     uint8_t checksum = 0xFF - ( (DXL_BROADCAST + length + DXL_SYNC_WRITE + address + servo_length + sum) % 256 );
01585 
01586     // packet: FF  FF  ID LENGTH INSTRUCTION PARAM_1 ... CHECKSUM
01587     int packet_length = 4 + length;
01588     uint8_t packet[packet_length];
01589 
01590     packet[0] = 0xFF;
01591     packet[1] = 0xFF;
01592     packet[2] = DXL_BROADCAST;
01593     packet[3] = length;
01594     packet[4] = DXL_SYNC_WRITE;
01595     packet[5] = address;
01596     packet[6] = servo_length;
01597 
01598     for (size_t i = 0; i < data.size(); ++i)
01599     {
01600         for (size_t j = 0; j < data[i].size(); ++j)
01601         {
01602             packet[7+i*data[i].size()+j] = data[i][j];
01603         }
01604     }
01605 
01606     packet[packet_length-1] = checksum;
01607 
01608     pthread_mutex_lock(&serial_mutex_);
01609     bool success = writePacket(packet, packet_length);
01610     pthread_mutex_unlock(&serial_mutex_);
01611 
01612     return success;
01613 }
01614 
01615 bool DynamixelIO::waitForBytes(ssize_t n_bytes, uint16_t timeout_ms)
01616 {
01617     struct timespec ts_now;
01618     clock_gettime(CLOCK_REALTIME, &ts_now);
01619     double start_time_ms = ts_now.tv_sec * 1.0e3 + ts_now.tv_nsec / 1.0e6;
01620 
01621     // wait for response packet from the motor
01622     while (port_->BytesAvailableWait() < n_bytes)
01623     {
01624         clock_gettime(CLOCK_REALTIME, &ts_now);
01625         double current_time_ms = ts_now.tv_sec * 1.0e3 + ts_now.tv_nsec / 1.0e6;
01626 
01627         if (current_time_ms - start_time_ms > timeout_ms)
01628         {
01629             //printf("waitForBytes timed out trying to read %zd bytes in less than %dms\n", n_bytes, timeout_ms);
01630             return false;
01631         }
01632     }
01633 
01634     return true;
01635 }
01636 
01637 bool DynamixelIO::writePacket(const void* const buffer, size_t count)
01638 {
01639     port_->Flush();
01640     return (port_->Write(buffer, count) == (ssize_t) count);
01641 }
01642 
01643 bool DynamixelIO::readResponse(std::vector<uint8_t>& response)
01644 {
01645     struct timespec ts_now;
01646     clock_gettime(CLOCK_REALTIME, &ts_now);
01647     double current_time_sec = ts_now.tv_sec + ts_now.tv_nsec / 1.0e9;
01648     
01649     if (current_time_sec - last_reset_sec > 20)
01650     {
01651         read_count = 0;
01652         read_error_count = 0;
01653         last_reset_sec = current_time_sec;
01654     }
01655     
01656     ++read_count;
01657     
01658     static const uint16_t timeout_ms = 50;
01659     
01660     uint8_t buffer[1024];
01661     response.clear();
01662 
01663     // wait until we receive the header bytes and read them
01664     if (!waitForBytes(4, timeout_ms) || port_->Read(buffer, 4) != 4)
01665     {
01666         ++read_error_count;
01667         return false;
01668     }
01669     
01670     if (buffer[0] == 0xFF && buffer[1] == 0xFF)
01671     {
01672         response.push_back(buffer[0]);  // 0xFF
01673         response.push_back(buffer[1]);  // 0xFF
01674         response.push_back(buffer[2]);  // ID
01675         uint8_t n_bytes = buffer[3];    // Length
01676         response.push_back(n_bytes);
01677         
01678         // wait for and read the rest of response bytes
01679         if (!waitForBytes(n_bytes, timeout_ms) || port_->Read(buffer, n_bytes) != n_bytes)
01680         {
01681             ++read_error_count;
01682             response.clear();
01683             return false;
01684         }
01685         
01686         for (int i = 0; i < n_bytes; ++i)
01687         {
01688             response.push_back(buffer[i]);
01689         }
01690 
01691         // verify checksum
01692         uint8_t checksum = 0xFF;
01693         uint32_t sum = 0;
01694 
01695         for (uint32_t i = 2; i < response.size() - 1; ++i)
01696         {
01697             sum += response[i];
01698         }
01699 
01700         checksum -= sum % 256;
01701         
01702         if (checksum != response.back())
01703         {
01704             ++read_error_count;
01705             return false;
01706         }
01707 
01708         return true;
01709     }
01710 
01711     return false;
01712 }
01713 
01714 }


dynamixel_hardware_interface
Author(s): Antons Rebguns
autogenerated on Sun Oct 5 2014 23:33:10