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
RobotisRX28::DeviceList
std::list< unsigned char > DeviceList
List of servo IDs.
Definition:
rx28.h:47
fawkes
fawkes::Exception::print_trace
void print_trace()
Prints trace to stderr.
Definition:
exception.cpp:601
RobotisRX28
Definition:
rx28.h:43
fawkes::Exception
Definition:
exception.h:41
src
plugins
pantilt
qa
qa_rx28ptu.cpp
Generated by
1.8.17