Fawkes API  Fawkes Development Version
qa_rx28ptu.cpp
1 
2 /***************************************************************************
3  * qa_rx28ptu.cpp - QA for RX 28 PTU
4  *
5  * Created: Tue Jun 16 14:13:12 2009
6  * Copyright 2005-2009 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22  */
23 
24 /// @cond QA
25 
26 #include "../robotis/rx28.h"
27 #include <utils/time/tracker.h>
28 
29 #include <cstdio>
30 #include <unistd.h>
31 
32 using namespace fawkes;
33 
34 #define TEST_SERVO 2
35 
36 int
37 main(int argc, char **argv)
38 {
39  RobotisRX28 rx28("/dev/ttyUSB0");
40 
41  RobotisRX28::DeviceList devl = rx28.discover();
42 
43  if (devl.empty()) {
44  printf("No devices found\n");
45  } else {
46  for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
47  printf("Found servo with ID %d\n", *i);
48  }
49  }
50 
51  /*
52  rx28.set_status_return_level(RobotisRX28::BROADCAST_ID, RobotisRX28::SRL_RESPOND_READ);
53  rx28.set_return_delay_time(RobotisRX28::BROADCAST_ID, 0);
54  rx28.set_baudrate(RobotisRX28::BROADCAST_ID, 0x22);
55 
56  TimeTracker tt;
57  unsigned int ttc_goto = tt.add_class("Send goto");
58  unsigned int ttc_read_pos = tt.add_class("Read position");
59  unsigned int ttc_read_all = tt.add_class("Read all table values");
60  unsigned int ttc_start_read_all = tt.add_class("Starting to read all values");
61  unsigned int ttc_finish_read_all_1 = tt.add_class("1 Finishing reading all values");
62 
63  rx28.goto_position(2, 512);
64  rx28.set_compliance_values(1, 0, 96, 0, 96);
65 
66  rx28.goto_positions(2, 1, 230, 2, 300);
67 
68  return 0;
69 
70  for (unsigned int i = 0; i < 10; ++i) {
71  tt.ping_start(ttc_goto);
72  rx28.goto_position(1, 230);
73  tt.ping_end(ttc_goto);
74  sleep(1);
75  tt.ping_start(ttc_goto);
76  rx28.goto_position(1, 630);
77  tt.ping_end(ttc_goto);
78  sleep(1);
79 
80  tt.ping_start(ttc_read_all);
81  rx28.read_table_values(1);
82  tt.ping_end(ttc_read_all);
83 
84  tt.ping_start(ttc_read_pos);
85  rx28.get_position(1, true);
86  tt.ping_end(ttc_read_pos);
87 
88  tt.ping_start(ttc_start_read_all);
89  rx28.start_read_table_values(1);
90  tt.ping_end(ttc_start_read_all);
91  tt.ping_start(ttc_finish_read_all_1);
92  rx28.finish_read_table_values();
93  tt.ping_end(ttc_finish_read_all_1);
94  }
95 
96  tt.print_to_stdout();
97 
98 
99  //printf("Setting ID\n");
100  //rx28.set_id(1, 2);
101 
102  printf("Setting ID back\n");
103  rx28.set_id(2, 1);
104 
105  for (unsigned char i = 0; i <= 1; ++i) {
106  if (rx28.ping(i, 500)) {
107  printf("****************** RX28 ID %u alive\n", i);
108  } else {
109  //printf("RX28 ID %u dead (not connected?)\n", i);
110  }
111  }
112 
113  try {
114  rx28.read_table_values(1);
115  } catch (Exception &e) {
116  printf("Reading table values failed\n");
117  }
118 
119  try {
120  rx28.goto_position(2, 1000);
121  } catch (Exception &e) {
122  }
123 
124  sleep(2);
125  */
126 
127  try {
128  /*
129  rx28.goto_position(1, 430);
130  rx28.goto_position(2, 512);
131  sleep(1);
132 
133 
134  rx28.goto_position(1, 300);
135  rx28.goto_position(2, 300);
136 
137  sleep(3);
138 
139  rx28.goto_position(1, 700);
140  rx28.goto_position(2, 700);
141 
142  sleep(3);
143 
144  */
145 
146  //rx28.set_torque_enabled(0xFE, false);
147 
148  for (unsigned int i = 0; i < 5; ++i) {
149  try {
150  //rx28.goto_position(TEST_SERVO, 800);
151  //sleep(1);
152  //rx28.goto_position(TEST_SERVO, 400);
153  rx28.read_table_values(TEST_SERVO);
154  //sleep(1);
155  } catch (Exception &e) {
156  rx28.ping(TEST_SERVO);
157  e.print_trace();
158  }
159  }
160  // for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
161  // unsigned int angle_cw_limit, angle_ccw_limit, down_calib, up_calib;
162  // unsigned char voltage_low, voltage_high;
163  // unsigned char compl_cw_margin, compl_cw_slope, compl_ccw_margin, compl_ccw_slope;
164  // rx28.get_angle_limits(*i, angle_cw_limit, angle_ccw_limit);
165  // rx28.get_voltage_limits(*i, voltage_low, voltage_high);
166  // rx28.get_calibration(*i, down_calib, up_calib);
167  // rx28.get_compliance_values(*i, compl_cw_margin, compl_cw_slope, compl_ccw_margin, compl_ccw_slope);
168 
169  // printf("Servo %03u, model number: %u\n", *i, rx28.get_model(*i));
170  // printf("Servo %03u, current position: %u\n", *i, rx28.get_position(*i));
171  // printf("Servo %03u, firmware version: %u\n", *i, rx28.get_firmware_version(*i));
172  // printf("Servo %03u, baudrate: %u\n", *i, rx28.get_baudrate(*i));
173  // printf("Servo %03u, delay time: %u\n", *i, rx28.get_delay_time(*i));
174  // printf("Servo %03u, angle limits: CW: %u CCW: %u\n", *i, angle_cw_limit, angle_ccw_limit);
175  // printf("Servo %03u, temperature limit: %u\n", *i, rx28.get_temperature_limit(*i));
176  // printf("Servo %03u, voltage limits: %u to %u\n", *i, voltage_low, voltage_high);
177  // printf("Servo %03u, max torque: %u\n", *i, rx28.get_max_torque(*i));
178  // printf("Servo %03u, status return level: %u\n", *i, rx28.get_status_return_level(*i));
179  // printf("Servo %03u, alarm LED: %u\n", *i, rx28.get_alarm_led(*i));
180  // printf("Servo %03u, alarm shutdown: %u\n", *i, rx28.get_alarm_shutdown(*i));
181  // printf("Servo %03u, calibration: %u to %u\n", *i, down_calib, up_calib);
182  // printf("Servo %03u, torque enabled: %s\n", *i, rx28.is_torque_enabled(*i) ? "Yes" : "No");
183  // printf("Servo %03u, LED enabled: %s\n", *i, rx28.is_led_enabled(*i) ? "Yes" : "No");
184  // printf("Servo %03u, compliance: CW_M: %u CW_S: %u CCW_M: %u CCW_S: %u\n", *i,
185  // compl_cw_margin, compl_cw_slope, compl_ccw_margin, compl_ccw_slope);
186  // printf("Servo %03u, goal position: %u\n", *i, rx28.get_goal_position(*i));
187  // printf("Servo %03u, goal speed: %u\n", *i, rx28.get_goal_speed(*i));
188  // printf("Servo %03u, torque limit: %u\n", *i, rx28.get_torque_limit(*i));
189  // printf("Servo %03u, speed: %u\n", *i, rx28.get_speed(*i));
190  // printf("Servo %03u, load: %u\n", *i, rx28.get_load(*i));
191  // printf("Servo %03u, voltage: %u\n", *i, rx28.get_voltage(*i));
192  // printf("Servo %03u, temperature: %u\n", *i, rx28.get_temperature(*i));
193  // printf("Servo %03u, moving: %s\n", *i, rx28.is_moving(*i) ? "Yes" : "No");
194  // printf("Servo %03u, Locked: %s\n", *i, rx28.is_locked(*i) ? "Yes" : "No");
195  // printf("Servo %03u, Punch: %u\n", *i, rx28.get_punch(*i));
196  // }
197  } catch (Exception &e) {
198  e.print_trace();
199  }
200 
201  /*
202  sleep(2);
203 
204  try {
205  rx28.goto_position(2, 800);
206  } catch (Exception &e) {
207  }
208  */
209 
210 // std::list<unsigned char> disc = rx28.discover();
211 
212 // if (disc.empty()) {
213 // printf("No devices found\n");
214 // } else {
215 // for (std::list<unsigned char>::iterator i = disc.begin(); i != disc.end(); ++i) {
216 // printf("Found servo with ID %d\n", *i);
217 // }
218 // }
219 
220  return 0;
221 }
222 
223 /// @endcond
std::list< unsigned char > DeviceList
List of servo IDs.
Definition: rx28.h:46
Fawkes library namespace.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void print_trace()
Prints trace to stderr.
Definition: exception.cpp:619
Class to access a chain of Robotis RX28 servos.
Definition: rx28.h:42