rx28.h

00001 
00002 /***************************************************************************
00003  *  rx28.h - Class for accessing Robotis RX28 servos
00004  *
00005  *  Created: Tue Jun 16 11:09:32 2009 (based on visca.h)
00006  *  Copyright  2005-2009  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version. A runtime exception applies to
00014  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00022  */
00023 
00024 #ifndef __PLUGINS_PANTILT_ROBOTIS_RX28_H_
00025 #define __PLUGINS_PANTILT_ROBOTIS_RX28_H_
00026 
00027 #include <core/exception.h>
00028 
00029 #ifdef USE_TIMETRACKER
00030 #  warning Visca time tracker enabled
00031 #  include <utils/timetracker.h>
00032 #  include <fstream>
00033 #endif
00034 
00035 #include <list>
00036 #include <cstddef>
00037 #include <cstdio>
00038 
00039 #define RX28_CONTROL_TABLE_LENGTH 0x32
00040 #define RX28_MAX_NUM_SERVOS       254
00041 
00042 class RobotisRX28
00043 {
00044  public:
00045   /** List of servo IDs. */
00046   typedef std::list<unsigned char> DeviceList;
00047 
00048   RobotisRX28(const char *device_file, unsigned int default_timeout_ms = 30);
00049   ~RobotisRX28();
00050 
00051   void         open();
00052   void         close();
00053 
00054   bool         ping(unsigned char id, unsigned int timeout_ms = 100);
00055   DeviceList   discover(unsigned int total_timeout_ms = 50);
00056 
00057 
00058   void write_table_value(unsigned char id, unsigned char addr,
00059                          unsigned int value, bool double_byte = false);
00060   void write_table_values(unsigned char id, unsigned char start_addr,
00061                           unsigned char *values, unsigned int num_values);
00062   void read_table_values(unsigned char id);
00063   void read_table_value(unsigned char id,
00064                         unsigned char addr, unsigned char read_length);
00065   void          start_read_table_values(unsigned char id);
00066   void          finish_read_table_values();
00067 
00068   void          goto_position(unsigned char id, unsigned int value);
00069   void          goto_positions(unsigned int num_positions, ...);
00070 
00071   unsigned int  get_model(unsigned char id, bool refresh = false);
00072   unsigned int  get_position(unsigned char id, bool refresh = false);
00073   unsigned char get_firmware_version(unsigned char id, bool refresh = false);
00074   unsigned char get_baudrate(unsigned char id, bool refresh = false);
00075   unsigned char get_delay_time(unsigned char id, bool refresh = false);
00076   void          get_angle_limits(unsigned char id,
00077                                  unsigned int &cw_limit, unsigned int &ccw_limit,
00078                                  bool refresh = false);
00079   unsigned char get_temperature_limit(unsigned char id, bool refresh = false);
00080   void          get_voltage_limits(unsigned char id,
00081                                    unsigned char &low, unsigned char &high,
00082                                    bool refresh = false);
00083   unsigned int  get_max_torque(unsigned char id, bool refresh = false);
00084   unsigned char get_status_return_level(unsigned char id, bool refresh = false);
00085   unsigned char get_alarm_led(unsigned char id, bool refresh = false);
00086   unsigned char get_alarm_shutdown(unsigned char id, bool refresh = false);
00087   void          get_calibration(unsigned char id,
00088                                 unsigned int &down_calib, unsigned int &up_calib,
00089                                 bool refresh = false);
00090   bool          is_torque_enabled(unsigned char id, bool refresh = false);
00091   bool          is_led_enabled(unsigned char id, bool refresh = false);
00092   void          get_compliance_values(unsigned char id,
00093                                       unsigned char &cw_margin, unsigned char &cw_slope,
00094                                       unsigned char &ccw_margin, unsigned char &ccw_slope,
00095                                       bool refresh = false);
00096   unsigned int  get_goal_position(unsigned char id, bool refresh = false);
00097   unsigned int  get_goal_speed(unsigned char id, bool refresh = false);
00098   float         get_max_supported_speed(unsigned char id, bool refresh = false);
00099   unsigned int  get_torque_limit(unsigned char id, bool refresh = false);
00100   unsigned int  get_speed(unsigned char id, bool refresh = false);
00101   unsigned int  get_load(unsigned char id, bool refresh = false);
00102   unsigned char get_voltage(unsigned char id, bool refresh = false);
00103   unsigned char get_temperature(unsigned char id, bool refresh = false);
00104   bool          is_moving(unsigned char id, bool refresh = false);
00105   bool          is_locked(unsigned char id, bool refresh = false);
00106   unsigned int  get_punch(unsigned char id, bool refresh = false);
00107 
00108   void          set_id(unsigned char id, unsigned char new_id);
00109   void          set_baudrate(unsigned char id, unsigned char baudrate);
00110   void          set_return_delay_time(unsigned char id, unsigned char return_delay_time);
00111   void          set_angle_limits(unsigned char id,
00112                                  unsigned int cw_limit, unsigned int ccw_limit);
00113   void          set_temperature_limit(unsigned char id, unsigned char temp_limit);
00114   void          set_voltage_limits(unsigned char id, unsigned char low, unsigned char high);
00115   void          set_max_torque(unsigned char id, unsigned int max_torque);
00116   void          set_status_return_level(unsigned char id, unsigned char status_return_level);
00117   void          set_alarm_led(unsigned char id, unsigned char alarm_led);
00118   void          set_alarm_shutdown(unsigned char id, unsigned char alarm_shutdown);
00119   void          set_torque_enabled(unsigned char id, bool enabled);
00120   void          set_torques_enabled(bool enabled, unsigned char num_servos, ...);
00121   void          set_led_enabled(unsigned char id, bool enabled);
00122   void          set_compliance_values(unsigned char id,
00123                                       unsigned char cw_margin, unsigned char cw_slope,
00124                                       unsigned char ccw_margin, unsigned char ccw_slope);
00125   void          set_goal_speed(unsigned char id, unsigned int goal_speed);
00126   void          set_goal_speeds(unsigned int num_servos, ...);
00127   void          set_torque_limit(unsigned char id, unsigned int torque_limit);
00128   void          lock_config(unsigned char id);
00129   void          set_punch(unsigned char id, unsigned int punch);
00130 
00131   bool          data_available();
00132 
00133   // Status return level
00134   static const unsigned char SRL_RESPOND_NONE;
00135   static const unsigned char SRL_RESPOND_READ;
00136   static const unsigned char SRL_RESPOND_ALL;
00137 
00138   static const unsigned char BROADCAST_ID;
00139   static const unsigned int  MAX_POSITION;
00140   static const unsigned int  CENTER_POSITION;
00141   static const float         MAX_ANGLE_DEG;
00142   static const float         MAX_ANGLE_RAD;
00143   static const float         RAD_PER_POS_TICK;
00144   static const float         POS_TICKS_PER_RAD;
00145   static const float         SEC_PER_60DEG_12V;
00146   static const float         SEC_PER_60DEG_16V;
00147   static const unsigned int  MAX_SPEED;
00148 
00149   // Parameter entry offsets
00150   static const unsigned char P_MODEL_NUMBER_L;
00151   static const unsigned char P_MODEL_NUMBER_H;
00152   static const unsigned char P_VERSION;
00153   static const unsigned char P_ID;
00154   static const unsigned char P_BAUD_RATE;
00155   static const unsigned char P_RETURN_DELAY_TIME;
00156   static const unsigned char P_CW_ANGLE_LIMIT_L;
00157   static const unsigned char P_CW_ANGLE_LIMIT_H;
00158   static const unsigned char P_CCW_ANGLE_LIMIT_L;
00159   static const unsigned char P_CCW_ANGLE_LIMIT_H;
00160   static const unsigned char P_SYSTEM_DATA2;
00161   static const unsigned char P_LIMIT_TEMPERATURE;
00162   static const unsigned char P_DOWN_LIMIT_VOLTAGE;
00163   static const unsigned char P_UP_LIMIT_VOLTAGE;
00164   static const unsigned char P_MAX_TORQUE_L;
00165   static const unsigned char P_MAX_TORQUE_H;
00166   static const unsigned char P_RETURN_LEVEL;
00167   static const unsigned char P_ALARM_LED;
00168   static const unsigned char P_ALARM_SHUTDOWN;
00169   static const unsigned char P_OPERATING_MODE;
00170   static const unsigned char P_DOWN_CALIBRATION_L;
00171   static const unsigned char P_DOWN_CALIBRATION_H;
00172   static const unsigned char P_UP_CALIBRATION_L;
00173   static const unsigned char P_UP_CALIBRATION_H;
00174 
00175   static const unsigned char P_TORQUE_ENABLE;
00176   static const unsigned char P_LED;
00177   static const unsigned char P_CW_COMPLIANCE_MARGIN;
00178   static const unsigned char P_CCW_COMPLIANCE_MARGIN;
00179   static const unsigned char P_CW_COMPLIANCE_SLOPE;
00180   static const unsigned char P_CCW_COMPLIANCE_SLOPE;
00181   static const unsigned char P_GOAL_POSITION_L;
00182   static const unsigned char P_GOAL_POSITION_H;
00183   static const unsigned char P_GOAL_SPEED_L;
00184   static const unsigned char P_GOAL_SPEED_H;
00185   static const unsigned char P_TORQUE_LIMIT_L;
00186   static const unsigned char P_TORQUE_LIMIT_H;
00187   static const unsigned char P_PRESENT_POSITION_L;
00188   static const unsigned char P_PRESENT_POSITION_H;
00189   static const unsigned char P_PRESENT_SPEED_L;
00190   static const unsigned char P_PRESENT_SPEED_H;
00191   static const unsigned char P_PRESENT_LOAD_L;
00192   static const unsigned char P_PRESENT_LOAD_H;
00193   static const unsigned char P_PRESENT_VOLTAGE;
00194   static const unsigned char P_PRESENT_TEMPERATURE;
00195   static const unsigned char P_REGISTERED_INSTRUCTION;
00196   static const unsigned char P_PAUSE_TIME;
00197   static const unsigned char P_MOVING;
00198   static const unsigned char P_LOCK;
00199   static const unsigned char P_PUNCH_L;
00200   static const unsigned char P_PUNCH_H;
00201 
00202  private:
00203   // Instructions
00204   static const unsigned char INST_PING;
00205   static const unsigned char INST_READ;
00206   static const unsigned char INST_WRITE;
00207   static const unsigned char INST_REG_WRITE;
00208   static const unsigned char INST_ACTION;
00209   static const unsigned char INST_RESET;
00210   static const unsigned char INST_DIGITAL_RESET;
00211   static const unsigned char INST_SYSTEM_READ;
00212   static const unsigned char INST_SYSTEM_WRITE;
00213   static const unsigned char INST_SYNC_WRITE;
00214   static const unsigned char INST_SYNC_REG_WRITE;
00215 
00216   // Packet offsets
00217   static const unsigned char PACKET_OFFSET_ID;
00218   static const unsigned char PACKET_OFFSET_LENGTH;
00219   static const unsigned char PACKET_OFFSET_INST;
00220   static const unsigned char PACKET_OFFSET_PARAM;
00221   static const unsigned char PACKET_OFFSET_ERROR;
00222 
00223  private:
00224   unsigned char calc_checksum(const unsigned char id, const unsigned char instruction,
00225                               const unsigned char *params, const unsigned char plength);
00226   void send(const unsigned char id, const unsigned char instruction,
00227             const unsigned char *params, const unsigned char plength);
00228   void recv(unsigned int timeout_ms = 0xFFFFFFFF);
00229   void assert_valid_id(unsigned char id);
00230   unsigned int merge_twobyte_value(unsigned int id,
00231                                    unsigned char ind_l, unsigned char ind_h);
00232   unsigned int get_value(unsigned int id, bool refresh,
00233                          unsigned int ind_l, unsigned int ind_h = 0xFFFFFFFF);
00234   bool inline responds_read(unsigned int id)
00235   {
00236     
00237     return ((__control_table[id][P_RETURN_LEVEL] == SRL_RESPOND_READ) ||
00238             (__control_table[id][P_RETURN_LEVEL] == SRL_RESPOND_ALL));
00239   }
00240 
00241   bool inline responds_all(unsigned int id)
00242   {
00243     return (__control_table[id][P_RETURN_LEVEL] == SRL_RESPOND_ALL);
00244   }
00245 
00246   int           __fd;
00247   char         *__device_file;
00248   unsigned int  __default_timeout_ms;
00249 
00250   unsigned char __obuffer[260];
00251   unsigned char __ibuffer[260];
00252 
00253   int           __obuffer_length;
00254   int           __ibuffer_length;
00255 
00256   char          __control_table[RX28_MAX_NUM_SERVOS][RX28_CONTROL_TABLE_LENGTH];
00257 
00258 #ifdef TIMETRACKER_VISCA
00259   fawkes::TimeTracker    *tracker;
00260   std::ofstream   track_file;
00261   unsigned int    ttcls_pantilt_get_send;
00262   unsigned int    ttcls_pantilt_get_read;
00263   unsigned int    ttcls_pantilt_get_handle;
00264   unsigned int    ttcls_pantilt_get_interpret;
00265 #endif
00266 
00267 };
00268 
00269 
00270 
00271 #endif

Generated on Tue Feb 22 13:31:29 2011 for Fawkes API by  doxygen 1.4.7