Fawkes API  Fawkes Development Version
qa_rx28ptu.cpp
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