00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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
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
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
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
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
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