Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * qa_rx28ptu.cpp - QA for RX 28 PTU 00004 * 00005 * Created: Tue Jun 16 14:13:12 2009 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 /// @cond QA 00025 00026 #include "../robotis/rx28.h" 00027 #include <utils/time/tracker.h> 00028 00029 #include <cstdio> 00030 #include <unistd.h> 00031 00032 using namespace fawkes; 00033 00034 #define TEST_SERVO 2 00035 00036 int 00037 main(int argc, char **argv) 00038 { 00039 RobotisRX28 rx28("/dev/ttyUSB0"); 00040 00041 RobotisRX28::DeviceList devl = rx28.discover(); 00042 00043 if (devl.empty()) { 00044 printf("No devices found\n"); 00045 } else { 00046 for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) { 00047 printf("Found servo with ID %d\n", *i); 00048 } 00049 } 00050 00051 /* 00052 rx28.set_status_return_level(RobotisRX28::BROADCAST_ID, RobotisRX28::SRL_RESPOND_READ); 00053 rx28.set_return_delay_time(RobotisRX28::BROADCAST_ID, 0); 00054 rx28.set_baudrate(RobotisRX28::BROADCAST_ID, 0x22); 00055 00056 TimeTracker tt; 00057 unsigned int ttc_goto = tt.add_class("Send goto"); 00058 unsigned int ttc_read_pos = tt.add_class("Read position"); 00059 unsigned int ttc_read_all = tt.add_class("Read all table values"); 00060 unsigned int ttc_start_read_all = tt.add_class("Starting to read all values"); 00061 unsigned int ttc_finish_read_all_1 = tt.add_class("1 Finishing reading all values"); 00062 00063 rx28.goto_position(2, 512); 00064 rx28.set_compliance_values(1, 0, 96, 0, 96); 00065 00066 rx28.goto_positions(2, 1, 230, 2, 300); 00067 00068 return 0; 00069 00070 for (unsigned int i = 0; i < 10; ++i) { 00071 tt.ping_start(ttc_goto); 00072 rx28.goto_position(1, 230); 00073 tt.ping_end(ttc_goto); 00074 sleep(1); 00075 tt.ping_start(ttc_goto); 00076 rx28.goto_position(1, 630); 00077 tt.ping_end(ttc_goto); 00078 sleep(1); 00079 00080 tt.ping_start(ttc_read_all); 00081 rx28.read_table_values(1); 00082 tt.ping_end(ttc_read_all); 00083 00084 tt.ping_start(ttc_read_pos); 00085 rx28.get_position(1, true); 00086 tt.ping_end(ttc_read_pos); 00087 00088 tt.ping_start(ttc_start_read_all); 00089 rx28.start_read_table_values(1); 00090 tt.ping_end(ttc_start_read_all); 00091 tt.ping_start(ttc_finish_read_all_1); 00092 rx28.finish_read_table_values(); 00093 tt.ping_end(ttc_finish_read_all_1); 00094 } 00095 00096 tt.print_to_stdout(); 00097 00098 00099 //printf("Setting ID\n"); 00100 //rx28.set_id(1, 2); 00101 00102 printf("Setting ID back\n"); 00103 rx28.set_id(2, 1); 00104 00105 for (unsigned char i = 0; i <= 1; ++i) { 00106 if (rx28.ping(i, 500)) { 00107 printf("****************** RX28 ID %u alive\n", i); 00108 } else { 00109 //printf("RX28 ID %u dead (not connected?)\n", i); 00110 } 00111 } 00112 00113 try { 00114 rx28.read_table_values(1); 00115 } catch (Exception &e) { 00116 printf("Reading table values failed\n"); 00117 } 00118 00119 try { 00120 rx28.goto_position(2, 1000); 00121 } catch (Exception &e) { 00122 } 00123 00124 sleep(2); 00125 */ 00126 00127 try { 00128 /* 00129 rx28.goto_position(1, 430); 00130 rx28.goto_position(2, 512); 00131 sleep(1); 00132 00133 00134 rx28.goto_position(1, 300); 00135 rx28.goto_position(2, 300); 00136 00137 sleep(3); 00138 00139 rx28.goto_position(1, 700); 00140 rx28.goto_position(2, 700); 00141 00142 sleep(3); 00143 00144 */ 00145 00146 //rx28.set_torque_enabled(0xFE, false); 00147 00148 for (unsigned int i = 0; i < 5; ++i) { 00149 try { 00150 //rx28.goto_position(TEST_SERVO, 800); 00151 //sleep(1); 00152 //rx28.goto_position(TEST_SERVO, 400); 00153 rx28.read_table_values(TEST_SERVO); 00154 //sleep(1); 00155 } catch (Exception &e) { 00156 rx28.ping(TEST_SERVO); 00157 e.print_trace(); 00158 } 00159 } 00160 // for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) { 00161 // unsigned int angle_cw_limit, angle_ccw_limit, down_calib, up_calib; 00162 // unsigned char voltage_low, voltage_high; 00163 // unsigned char compl_cw_margin, compl_cw_slope, compl_ccw_margin, compl_ccw_slope; 00164 // rx28.get_angle_limits(*i, angle_cw_limit, angle_ccw_limit); 00165 // rx28.get_voltage_limits(*i, voltage_low, voltage_high); 00166 // rx28.get_calibration(*i, down_calib, up_calib); 00167 // rx28.get_compliance_values(*i, compl_cw_margin, compl_cw_slope, compl_ccw_margin, compl_ccw_slope); 00168 00169 // printf("Servo %03u, model number: %u\n", *i, rx28.get_model(*i)); 00170 // printf("Servo %03u, current position: %u\n", *i, rx28.get_position(*i)); 00171 // printf("Servo %03u, firmware version: %u\n", *i, rx28.get_firmware_version(*i)); 00172 // printf("Servo %03u, baudrate: %u\n", *i, rx28.get_baudrate(*i)); 00173 // printf("Servo %03u, delay time: %u\n", *i, rx28.get_delay_time(*i)); 00174 // printf("Servo %03u, angle limits: CW: %u CCW: %u\n", *i, angle_cw_limit, angle_ccw_limit); 00175 // printf("Servo %03u, temperature limit: %u\n", *i, rx28.get_temperature_limit(*i)); 00176 // printf("Servo %03u, voltage limits: %u to %u\n", *i, voltage_low, voltage_high); 00177 // printf("Servo %03u, max torque: %u\n", *i, rx28.get_max_torque(*i)); 00178 // printf("Servo %03u, status return level: %u\n", *i, rx28.get_status_return_level(*i)); 00179 // printf("Servo %03u, alarm LED: %u\n", *i, rx28.get_alarm_led(*i)); 00180 // printf("Servo %03u, alarm shutdown: %u\n", *i, rx28.get_alarm_shutdown(*i)); 00181 // printf("Servo %03u, calibration: %u to %u\n", *i, down_calib, up_calib); 00182 // printf("Servo %03u, torque enabled: %s\n", *i, rx28.is_torque_enabled(*i) ? "Yes" : "No"); 00183 // printf("Servo %03u, LED enabled: %s\n", *i, rx28.is_led_enabled(*i) ? "Yes" : "No"); 00184 // printf("Servo %03u, compliance: CW_M: %u CW_S: %u CCW_M: %u CCW_S: %u\n", *i, 00185 // compl_cw_margin, compl_cw_slope, compl_ccw_margin, compl_ccw_slope); 00186 // printf("Servo %03u, goal position: %u\n", *i, rx28.get_goal_position(*i)); 00187 // printf("Servo %03u, goal speed: %u\n", *i, rx28.get_goal_speed(*i)); 00188 // printf("Servo %03u, torque limit: %u\n", *i, rx28.get_torque_limit(*i)); 00189 // printf("Servo %03u, speed: %u\n", *i, rx28.get_speed(*i)); 00190 // printf("Servo %03u, load: %u\n", *i, rx28.get_load(*i)); 00191 // printf("Servo %03u, voltage: %u\n", *i, rx28.get_voltage(*i)); 00192 // printf("Servo %03u, temperature: %u\n", *i, rx28.get_temperature(*i)); 00193 // printf("Servo %03u, moving: %s\n", *i, rx28.is_moving(*i) ? "Yes" : "No"); 00194 // printf("Servo %03u, Locked: %s\n", *i, rx28.is_locked(*i) ? "Yes" : "No"); 00195 // printf("Servo %03u, Punch: %u\n", *i, rx28.get_punch(*i)); 00196 // } 00197 } catch (Exception &e) { 00198 e.print_trace(); 00199 } 00200 00201 /* 00202 sleep(2); 00203 00204 try { 00205 rx28.goto_position(2, 800); 00206 } catch (Exception &e) { 00207 } 00208 */ 00209 00210 // std::list<unsigned char> disc = rx28.discover(); 00211 00212 // if (disc.empty()) { 00213 // printf("No devices found\n"); 00214 // } else { 00215 // for (std::list<unsigned char>::iterator i = disc.begin(); i != disc.end(); ++i) { 00216 // printf("Found servo with ID %d\n", *i); 00217 // } 00218 // } 00219 00220 return 0; 00221 } 00222 00223 /// @endcond