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
33using namespace fawkes;
34
35#define TEST_SERVO 2
36
37int
38main(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.