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 
28 #include <utils/time/tracker.h>
29 
30 #include <cstdio>
31 #include <unistd.h>
32 
33 using namespace fawkes;
34 
35 #define TEST_SERVO 2
36 
37 int
38 main(int argc, char **argv)
39 {
40  RobotisRX28 rx28("/dev/ttyUSB0");
41 
42  RobotisRX28::DeviceList devl = rx28.discover();
43 
44  if (devl.empty()) {
45  printf("No devices found\n");
46  } else {
47  for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
48  printf("Found servo with ID %d\n", *i);
49  }
50  }
51 
52  /*
53  rx28.set_status_return_level(RobotisRX28::BROADCAST_ID, RobotisRX28::SRL_RESPOND_READ);
54  rx28.set_return_delay_time(RobotisRX28::BROADCAST_ID, 0);
55  rx28.set_baudrate(RobotisRX28::BROADCAST_ID, 0x22);
56 
57  TimeTracker tt;
58  unsigned int ttc_goto = tt.add_class("Send goto");
59  unsigned int ttc_read_pos = tt.add_class("Read position");
60  unsigned int ttc_read_all = tt.add_class("Read all table values");
61  unsigned int ttc_start_read_all = tt.add_class("Starting to read all values");
62  unsigned int ttc_finish_read_all_1 = tt.add_class("1 Finishing reading all values");
63 
64  rx28.goto_position(2, 512);
65  rx28.set_compliance_values(1, 0, 96, 0, 96);
66 
67  rx28.goto_positions(2, 1, 230, 2, 300);
68 
69  return 0;
70 
71  for (unsigned int i = 0; i < 10; ++i) {
72  tt.ping_start(ttc_goto);
73  rx28.goto_position(1, 230);
74  tt.ping_end(ttc_goto);
75  sleep(1);
76  tt.ping_start(ttc_goto);
77  rx28.goto_position(1, 630);
78  tt.ping_end(ttc_goto);
79  sleep(1);
80 
81  tt.ping_start(ttc_read_all);
82  rx28.read_table_values(1);
83  tt.ping_end(ttc_read_all);
84 
85  tt.ping_start(ttc_read_pos);
86  rx28.get_position(1, true);
87  tt.ping_end(ttc_read_pos);
88 
89  tt.ping_start(ttc_start_read_all);
90  rx28.start_read_table_values(1);
91  tt.ping_end(ttc_start_read_all);
92  tt.ping_start(ttc_finish_read_all_1);
93  rx28.finish_read_table_values();
94  tt.ping_end(ttc_finish_read_all_1);
95  }
96 
97  tt.print_to_stdout();
98 
99 
100  //printf("Setting ID\n");
101  //rx28.set_id(1, 2);
102 
103  printf("Setting ID back\n");
104  rx28.set_id(2, 1);
105 
106  for (unsigned char i = 0; i <= 1; ++i) {
107  if (rx28.ping(i, 500)) {
108  printf("****************** RX28 ID %u alive\n", i);
109  } else {
110  //printf("RX28 ID %u dead (not connected?)\n", i);
111  }
112  }
113 
114  try {
115  rx28.read_table_values(1);
116  } catch (Exception &e) {
117  printf("Reading table values failed\n");
118  }
119 
120  try {
121  rx28.goto_position(2, 1000);
122  } catch (Exception &e) {
123  }
124 
125  sleep(2);
126  */
127 
128  try {
129  /*
130  rx28.goto_position(1, 430);
131  rx28.goto_position(2, 512);
132  sleep(1);
133 
134 
135  rx28.goto_position(1, 300);
136  rx28.goto_position(2, 300);
137 
138  sleep(3);
139 
140  rx28.goto_position(1, 700);
141  rx28.goto_position(2, 700);
142 
143  sleep(3);
144 
145  */
146 
147  //rx28.set_torque_enabled(0xFE, false);
148 
149  for (unsigned int i = 0; i < 5; ++i) {
150  try {
151  //rx28.goto_position(TEST_SERVO, 800);
152  //sleep(1);
153  //rx28.goto_position(TEST_SERVO, 400);
154  rx28.read_table_values(TEST_SERVO);
155  //sleep(1);
156  } catch (Exception &e) {
157  rx28.ping(TEST_SERVO);
158  e.print_trace();
159  }
160  }
161  // for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
162  // unsigned int angle_cw_limit, angle_ccw_limit, down_calib, up_calib;
163  // unsigned char voltage_low, voltage_high;
164  // unsigned char compl_cw_margin, compl_cw_slope, compl_ccw_margin, compl_ccw_slope;
165  // rx28.get_angle_limits(*i, angle_cw_limit, angle_ccw_limit);
166  // rx28.get_voltage_limits(*i, voltage_low, voltage_high);
167  // rx28.get_calibration(*i, down_calib, up_calib);
168  // rx28.get_compliance_values(*i, compl_cw_margin, compl_cw_slope, compl_ccw_margin, compl_ccw_slope);
169 
170  // printf("Servo %03u, model number: %u\n", *i, rx28.get_model(*i));
171  // printf("Servo %03u, current position: %u\n", *i, rx28.get_position(*i));
172  // printf("Servo %03u, firmware version: %u\n", *i, rx28.get_firmware_version(*i));
173  // printf("Servo %03u, baudrate: %u\n", *i, rx28.get_baudrate(*i));
174  // printf("Servo %03u, delay time: %u\n", *i, rx28.get_delay_time(*i));
175  // printf("Servo %03u, angle limits: CW: %u CCW: %u\n", *i, angle_cw_limit, angle_ccw_limit);
176  // printf("Servo %03u, temperature limit: %u\n", *i, rx28.get_temperature_limit(*i));
177  // printf("Servo %03u, voltage limits: %u to %u\n", *i, voltage_low, voltage_high);
178  // printf("Servo %03u, max torque: %u\n", *i, rx28.get_max_torque(*i));
179  // printf("Servo %03u, status return level: %u\n", *i, rx28.get_status_return_level(*i));
180  // printf("Servo %03u, alarm LED: %u\n", *i, rx28.get_alarm_led(*i));
181  // printf("Servo %03u, alarm shutdown: %u\n", *i, rx28.get_alarm_shutdown(*i));
182  // printf("Servo %03u, calibration: %u to %u\n", *i, down_calib, up_calib);
183  // printf("Servo %03u, torque enabled: %s\n", *i, rx28.is_torque_enabled(*i) ? "Yes" : "No");
184  // printf("Servo %03u, LED enabled: %s\n", *i, rx28.is_led_enabled(*i) ? "Yes" : "No");
185  // printf("Servo %03u, compliance: CW_M: %u CW_S: %u CCW_M: %u CCW_S: %u\n", *i,
186  // compl_cw_margin, compl_cw_slope, compl_ccw_margin, compl_ccw_slope);
187  // printf("Servo %03u, goal position: %u\n", *i, rx28.get_goal_position(*i));
188  // printf("Servo %03u, goal speed: %u\n", *i, rx28.get_goal_speed(*i));
189  // printf("Servo %03u, torque limit: %u\n", *i, rx28.get_torque_limit(*i));
190  // printf("Servo %03u, speed: %u\n", *i, rx28.get_speed(*i));
191  // printf("Servo %03u, load: %u\n", *i, rx28.get_load(*i));
192  // printf("Servo %03u, voltage: %u\n", *i, rx28.get_voltage(*i));
193  // printf("Servo %03u, temperature: %u\n", *i, rx28.get_temperature(*i));
194  // printf("Servo %03u, moving: %s\n", *i, rx28.is_moving(*i) ? "Yes" : "No");
195  // printf("Servo %03u, Locked: %s\n", *i, rx28.is_locked(*i) ? "Yes" : "No");
196  // printf("Servo %03u, Punch: %u\n", *i, rx28.get_punch(*i));
197  // }
198  } catch (Exception &e) {
199  e.print_trace();
200  }
201 
202  /*
203  sleep(2);
204 
205  try {
206  rx28.goto_position(2, 800);
207  } catch (Exception &e) {
208  }
209  */
210 
211  // std::list<unsigned char> disc = rx28.discover();
212 
213  // if (disc.empty()) {
214  // printf("No devices found\n");
215  // } else {
216  // for (std::list<unsigned char>::iterator i = disc.begin(); i != disc.end(); ++i) {
217  // printf("Found servo with ID %d\n", *i);
218  // }
219  // }
220 
221  return 0;
222 }
223 
224 /// @endcond
Class to access a chain of Robotis RX28 servos.
Definition: rx28.h:44
std::list< unsigned char > DeviceList
List of servo IDs.
Definition: rx28.h:47
Base class for exceptions in Fawkes.
Definition: exception.h:36
void print_trace() noexcept
Prints trace to stderr.
Definition: exception.cpp:601
Fawkes library namespace.