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 #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