Fawkes API
Fawkes Development Version
|
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(const unsigned char exp_length, 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