Fawkes API
Fawkes Development Version
Main Page
Related Pages
Modules
Namespaces
Classes
Files
File List
All
Classes
Namespaces
Functions
Variables
Typedefs
Enumerations
Enumerator
Friends
Groups
Pages
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
src
plugins
pantilt
qa
qa_rx28ptu.cpp
Generated by
1.8.1.1