dynamixel_hardware_interface: dynamixel

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 #ifndef DYNAMIXEL_IO_H__
00029 #define DYNAMIXEL_IO_H__
00030 
00031 #include <pthread.h>
00032 #include <stdint.h>
00033 
00034 #include <set>
00035 #include <map>
00036 #include <string>
00037 #include <vector>
00038 
00039 #include <gearbox/flexiport/port.h>
00040 
00041 namespace dynamixel_hardware_interface
00042 {
00043 
00044 typedef struct DynamixelDataStruct
00045 {
00046     uint16_t model_number;
00047     uint8_t  firmware_version;
00048     uint8_t  id;
00049     uint8_t  baud_rate;
00050     uint8_t  return_delay_time;
00051     uint16_t cw_angle_limit;
00052     uint16_t ccw_angle_limit;
00053     uint8_t  drive_mode;
00054     uint8_t  temperature_limit;
00055     uint8_t  voltage_limit_low;
00056     uint8_t  voltage_limit_high;
00057     uint16_t max_torque;
00058     uint8_t  return_level;
00059     uint8_t  alarm_led;
00060     uint8_t  alarm_shutdown;
00061     bool     torque_enabled;
00062     uint8_t  led;
00063     uint8_t  cw_compliance_margin;
00064     uint8_t  ccw_compliance_margin;
00065     uint8_t  cw_compliance_slope;
00066     uint8_t  ccw_compliance_slope;
00067     uint16_t target_position;
00068     int16_t  target_velocity;
00069     
00070     double   shutdown_error_time;
00071     std::string error;
00072     
00073 } DynamixelData;
00074 
00075 typedef struct DynamixelStatusStruct
00076 {
00077     double timestamp;
00078     
00079     uint16_t torque_limit;
00080     uint16_t position;
00081     int16_t  velocity;
00082     int16_t  load;
00083     uint8_t  voltage;
00084     uint8_t  temperature;
00085     bool     moving;
00086 
00087 } DynamixelStatus;
00088 
00089 
00090 class DynamixelIO
00091 {
00092 public:
00093     DynamixelIO(std::string device, std::string baud);
00094     ~DynamixelIO();
00095 
00096     long long unsigned int read_error_count;
00097     long long unsigned int read_count;
00098     double last_reset_sec;
00099     
00100     const DynamixelData* getCachedParameters(int servo_id);
00101     
00102     bool ping(int servo_id);
00103     bool resetOverloadError(int servo_id);
00104     
00105     // ****************************** GETTERS ******************************** //
00106     bool getModelNumber(int servo_id, uint16_t& model_number);
00107     bool getFirmwareVersion(int servo_id, uint8_t& firmware_version);
00108     bool getBaudRate(int servo_id, uint8_t& baud_rate);
00109     bool getReturnDelayTime(int servo_id, uint8_t& return_delay_time);
00110     
00111     bool getAngleLimits(int servo_id, uint16_t& cw_angle_limit, uint16_t& ccw_angle_limit);
00112     bool getCWAngleLimit(int servo_id, uint16_t& cw_angle);
00113     bool getCCWAngleLimit(int servo_id, uint16_t& ccw_angle);
00114     
00115     bool getVoltageLimits(int servo_id, float& min_voltage_limit, float& max_voltage_limit);
00116     bool getMinVoltageLimit(int servo_id, float& min_voltage_limit);
00117     bool getMaxVoltageLimit(int servo_id, float& max_voltage_limit);
00118     
00119     bool getTemperatureLimit(int servo_id, uint8_t& max_temperature);
00120     bool getMaxTorque(int servo_id, uint16_t& max_torque);
00121     bool getAlarmLed(int servo_id, uint8_t& alarm_led);
00122     bool getAlarmShutdown(int servo_id, uint8_t& alarm_shutdown);
00123     bool getTorqueEnable(int servo_id, bool& torque_enabled);
00124     bool getLedStatus(int servo_id, bool& led_enabled);
00125     
00126     bool getComplianceMargins(int servo_id, uint8_t& cw_compliance_margin, uint8_t& ccw_compliance_margin);
00127     bool getCWComplianceMargin(int servo_id, uint8_t& cw_compliance_margin);
00128     bool getCCWComplianceMargin(int servo_id, uint8_t& ccw_compliance_margin);
00129     
00130     bool getComplianceSlopes(int servo_id, uint8_t& cw_compliance_slope, uint8_t& ccw_compliance_slope);
00131     bool getCWComplianceSlope(int servo_id, uint8_t& cw_compliance_slope);
00132     bool getCCWComplianceSlope(int servo_id, uint8_t& ccw_compliance_slope);
00133     
00134     bool getTargetPosition(int servo_id, uint16_t& target_position);
00135     bool getTargetVelocity(int servo_id, int16_t& target_velocity);
00136     bool getTorqueLimit(int servo_id, uint16_t& torque_limit);
00137 
00138     bool getPosition(int servo_id, uint16_t& position);
00139     bool getVelocity(int servo_id, int16_t& velocity);
00140     bool getLoad(int servo_id, int16_t& load);
00141     bool getVoltage(int servo_id, float& voltage);
00142     bool getTemperature(int servo_id, uint8_t& temperature);
00143     bool getMoving(int servo_id, bool& is_moving);
00144     
00145     bool getFeedback(int servo_id, DynamixelStatus& status);
00146 
00147     // ****************************** SETTERS ******************************** //
00148     bool setId(int servo_id, uint8_t id);
00149     bool setBaudRate(int servo_id, uint8_t baud_rate);
00150     bool setReturnDelayTime(int servo_id, uint8_t return_delay_time);
00151     
00152     bool setAngleLimits(int servo_id, uint16_t cw_angle, uint16_t ccw_angle);
00153     bool setCWAngleLimit(int servo_id, uint16_t cw_angle);
00154     bool setCCWAngleLimit(int servo_id, uint16_t ccw_angle);
00155     
00156     bool setVoltageLimits(int servo_id, float min_voltage_limit, float max_voltage_limit);
00157     bool setMinVoltageLimit(int servo_id, float min_voltage_limit);
00158     bool setMaxVoltageLimit(int servo_id, float max_voltage_limit);
00159     
00160     bool setTemperatureLimit(int servo_id, uint8_t max_temperature);
00161     bool setMaxTorque(int servo_id, uint16_t max_torque);
00162     bool setAlarmLed(int servo_id, uint8_t alarm_led);
00163     bool setAlarmShutdown(int servo_id, uint8_t alarm_shutdown);
00164     bool setTorqueEnable(int servo_id, bool on);
00165     bool setLed(int servo_id, bool on);
00166     
00167     bool setComplianceMargins(int servo_id, uint8_t cw_margin, uint8_t ccw_margin);
00168     bool setCWComplianceMargin(int servo_id, uint8_t cw_margin);
00169     bool setCCWComplianceMargin(int servo_id, uint8_t ccw_margin);
00170 
00171     bool setComplianceSlopes(int servo_id, uint8_t cw_slope, uint8_t ccw_slope);
00172     bool setCWComplianceSlope(int servo_id, uint8_t cw_slope);
00173     bool setCCWComplianceSlope(int servo_id, uint8_t ccw_slope);
00174 
00175     bool setPosition(int servo_id, uint16_t position);
00176     bool setVelocity(int servo_id, int16_t velocity);
00177     bool setTorqueLimit(int servo_id, uint16_t torque_limit);
00178     
00179     // ************************* SYNC_WRITE METHODS *************************** //
00180     bool setMultiPosition(std::vector<std::vector<int> > value_pairs);
00181     bool setMultiVelocity(std::vector<std::vector<int> > value_pairs);
00182     bool setMultiPositionVelocity(std::vector<std::vector<int> > value_pairs);
00183     bool setMultiComplianceMargins(std::vector<std::vector<int> > value_pairs);
00184     bool setMultiComplianceSlopes(std::vector<std::vector<int> > value_pairs);
00185     bool setMultiTorqueEnabled(std::vector<std::vector<int> > value_pairs);
00186     bool setMultiTorqueLimit(std::vector<std::vector<int> > value_pairs);
00187     bool setMultiValues(std::vector<std::map<std::string, int> > value_maps);
00188     
00189 protected:
00190     std::map<int, DynamixelData*> cache_;
00191     std::set<int> connected_motors_;
00192 
00193     inline DynamixelData* findCachedParameters(int servo_id)
00194     {
00195         // this will either return an existing cache for servo_id or create new empty cahce and return that
00196         return cache_.insert(std::make_pair<int, DynamixelData*>(servo_id, new DynamixelData())).first->second;
00197     }
00198     
00199     bool updateCachedParameters(int servo_id, DynamixelData* data);
00200     void checkForErrors(int servo_id, uint8_t error_code, std::string command_failed);
00201 
00202     bool read(int servo_id,
00203               int address,
00204               int size,
00205               std::vector<uint8_t>& response);
00206 
00207     bool write(int servo_id,
00208                int address,
00209                const std::vector<uint8_t>& data,
00210                std::vector<uint8_t>& response);
00211 
00212     bool syncWrite(int address,
00213                    const std::vector<std::vector<uint8_t> >& data);
00214     
00215 private:
00216     flexiport::Port* port_;
00217     pthread_mutex_t serial_mutex_;
00218     
00219     bool waitForBytes(ssize_t n_bytes, uint16_t timeout_ms);
00220     
00221     bool writePacket(const void* const buffer, size_t count);
00222     bool readResponse(std::vector<uint8_t>& response);
00223 };
00224 
00225 }
00226 
00227 #endif