40 #include <visp3/core/vpConfig.h>
42 #if defined(VISP_HAVE_POLOLU) && defined(VISP_HAVE_THREADS)
49 #include <visp3/core/vpMath.h>
50 #include <visp3/robot/vpPololu.h>
52 void usage(
const char **argv,
int error,
const std::string &device,
int baudrate,
int channel,
53 unsigned short pwm_min,
unsigned short pwm_max,
float angle_min,
float angle_max)
55 std::cout <<
"Synopsis" << std::endl
56 <<
" " << argv[0] <<
" [--device <name>] [--baud <rate>] [--channel <number>] [--calibrate]"
57 <<
" [--range-pwm <min max> ] [--verbose, -v] [--help, -h]" << std::endl
59 std::cout <<
"Description" << std::endl
60 <<
" --device <name> Device name." << std::endl
61 <<
" Default: " << device << std::endl
63 <<
" --baud <rate> Serial link baud rate." << std::endl
64 <<
" Default: " << baudrate << std::endl
66 <<
" --channel <number> Channel to dial with." << std::endl
67 <<
" Default: " << channel << std::endl
69 <<
" --range-pwm <min max> Set PWM min and max values." << std::endl
70 <<
" You can use \"--calibrate\" to retrieve min and max pwm values."
71 <<
" Default: " << pwm_min <<
" " << pwm_max << std::endl
73 <<
" --range-angles <min max> Set angle min and max values (deg)." << std::endl
76 <<
" --verbose, -v Enable verbosity." << std::endl
78 <<
" --calibrate Start pwm calibration determining min and max admissible values." << std::endl
79 <<
" Once calibration done you can use \"--range-pwm <min max>\" option to set" << std::endl
80 <<
" the corresponding values" << std::endl
82 <<
" --help, -h Print this helper message." << std::endl
85 std::cout <<
"Error" << std::endl
87 <<
"Unsupported parameter " << argv[error] << std::endl;
91 int main(
int argc,
const char **argv)
94 std::string opt_device =
"COM4";
96 std::string opt_device =
"/dev/ttyACM0";
101 int opt_baudrate = 38400;
102 bool opt_verbose =
false;
103 bool opt_calibrate =
false;
104 unsigned short opt_pwm_min = 4000;
105 unsigned short opt_pwm_max = 8000;
106 float opt_angle_min =
static_cast<float>(
vpMath::rad(-45));
107 float opt_angle_max =
static_cast<float>(
vpMath::rad(45));
108 float opt_positioning_velocity =
static_cast<float>(
vpMath::rad(10));
109 float last_angle = 0;
112 for (
int i = 1; i < argc; i++) {
113 if (std::string(argv[i]) ==
"--device" && i + 1 < argc) {
114 opt_device = std::string(argv[i + 1]);
117 else if (std::string(argv[i]) ==
"--baud" && i + 1 < argc) {
118 opt_baudrate = std::atoi(argv[i + 1]);
121 else if (std::string(argv[i]) ==
"--channel" && i + 1 < argc) {
122 opt_channel = std::atoi(argv[i + 1]);
125 else if (std::string(argv[i]) ==
"--range-pwm" && i + 2 < argc) {
126 opt_pwm_min =
static_cast<unsigned short>(
vpMath::rad(std::atoi(argv[i + 1])));
127 opt_pwm_max =
static_cast<unsigned short>(
vpMath::rad(std::atoi(argv[i + 2])));
130 else if (std::string(argv[i]) ==
"--range-angles" && i + 2 < argc) {
131 opt_angle_min =
static_cast<float>(std::atof(argv[i + 1]));
132 opt_angle_max =
static_cast<float>(std::atof(argv[i + 2]));
135 else if (std::string(argv[i]) ==
"--calibrate") {
136 opt_calibrate =
true;
138 else if (std::string(argv[i]) ==
"--verbose" || std::string(argv[i]) ==
"-v") {
141 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
142 usage(argv, 0, opt_device, opt_baudrate, opt_channel, opt_pwm_min, opt_pwm_max, opt_angle_min, opt_angle_max);
146 usage(argv, i, opt_device, opt_baudrate, opt_channel, opt_pwm_min, opt_pwm_max, opt_angle_min, opt_angle_max);
153 vpPololu servo(opt_device, opt_baudrate, opt_channel, opt_verbose);
155 std::cout <<
"Pololu board is " << (servo.connected() ?
"connected" :
"disconnected") << std::endl;
158 std::cout <<
"Proceed to calibration to determine pwm min and max values..." << std::endl;
159 std::cout <<
"WARNING: Calibration will move the servo at channel " << opt_channel <<
"!" << std::endl;
160 std::cout <<
"Press Enter to move to min and max pwm positions..." << std::endl;
163 unsigned short pwm_min, pwm_max;
164 servo.calibrate(pwm_min, pwm_max);
165 std::cout <<
"Servo on channel " << opt_channel <<
" has pwm range [" << pwm_min <<
", " << pwm_max <<
"]" << std::endl;
169 servo.setPwmRange(opt_pwm_min, opt_pwm_max);
170 servo.setAngularRange(opt_angle_min, opt_angle_max);
173 servo.getRangePwm(opt_pwm_min, opt_pwm_max);
174 std::cout <<
"Position range (pwm): " << opt_pwm_min <<
" " << opt_pwm_max << std::endl;
175 servo.getRangeAngles(opt_angle_min, opt_angle_max);
176 std::cout <<
"Position range (deg): " <<
vpMath::deg(opt_angle_min) <<
" " <<
vpMath::deg(opt_angle_max) << std::endl;
179 std::cout <<
"Move to min position (pwm): " << opt_pwm_min <<
" at max velocity" << std::endl;
180 servo.setPwmPosition(opt_pwm_min, 0);
181 std::this_thread::sleep_for(std::chrono::seconds(3));
182 std::cout <<
"Servo reached position (pwm): " << servo.getPwmPosition() << std::endl;
184 std::cout <<
"Move to max position (pwm): " << opt_pwm_max <<
" at max velocity" << std::endl;
185 servo.setPwmPosition(opt_pwm_max, 0);
186 std::this_thread::sleep_for(std::chrono::seconds(3));
187 std::cout <<
"Servo reached position (pwm): " << servo.getPwmPosition() << std::endl;
190 std::cout <<
"Move to min position (deg): " <<
vpMath::deg(opt_angle_min) <<
" at max velocity" << std::endl;
191 servo.setAngularPosition(opt_angle_min, 0);
192 std::this_thread::sleep_for(std::chrono::seconds(3));
193 std::cout <<
"Servo reached position (deg): " <<
vpMath::deg(servo.getAngularPosition()) << std::endl;
195 std::cout <<
"Move to max position (deg): " <<
vpMath::deg(opt_angle_max) <<
" at max velocity" << std::endl;
196 servo.setAngularPosition(opt_angle_max, 0);
197 std::this_thread::sleep_for(std::chrono::seconds(3));
198 std::cout <<
"Servo reached position (deg): " <<
vpMath::deg(servo.getAngularPosition()) << std::endl;
201 std::cout <<
"Move to zero position (deg): " <<
vpMath::deg(0) <<
" at max velocity" << std::endl;
202 servo.setAngularPosition(0, 0);
203 std::this_thread::sleep_for(std::chrono::seconds(3));
204 last_angle = servo.getAngularPosition();
205 std::cout <<
"Servo reached position (deg): " <<
vpMath::deg(last_angle) << std::endl;
208 std::cout <<
"Move to min position (deg): " <<
vpMath::deg(opt_angle_min) <<
" at " <<
vpMath::deg(opt_positioning_velocity) <<
" deg/s" << std::endl;
209 servo.setAngularPosition(opt_angle_min, opt_positioning_velocity);
211 time_s =
static_cast<int>(std::abs((opt_angle_min - last_angle) / opt_positioning_velocity) + 2);
213 std::this_thread::sleep_for(std::chrono::seconds(time_s));
214 last_angle = servo.getAngularPosition();
215 std::cout <<
"Servo reached position (deg): " <<
vpMath::deg(last_angle) << std::endl;
217 std::cout <<
"Move to max position (deg): " <<
vpMath::deg(opt_angle_max) <<
" at " <<
vpMath::deg(opt_positioning_velocity) <<
" deg/s" << std::endl;
218 servo.setAngularPosition(opt_angle_max, opt_positioning_velocity);
220 time_s =
static_cast<int>(std::abs((opt_angle_max - last_angle) / opt_positioning_velocity) + 2);
221 std::this_thread::sleep_for(std::chrono::seconds(time_s));
222 last_angle = servo.getAngularPosition();
223 std::cout <<
"Servo reached position (deg): " <<
vpMath::deg(last_angle) << std::endl;
236 std::cout <<
"ViSP doesn't support Pololu 3rd party library" << std::endl;
error that can be emitted by ViSP classes.
const char * getMessage() const
static double rad(double deg)
static double deg(double rad)
Interface for the Pololu Maestro USB Servo Controllers.