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