39 #include <visp3/core/vpConfig.h>
41 #if defined(VISP_HAVE_POLOLU) && defined(VISP_HAVE_THREADS)
48 #include <visp3/core/vpMath.h>
49 #include <visp3/robot/vpPololu.h>
51 void usage(
const char **argv,
int error,
const std::string &device,
int baudrate,
int channel,
52 unsigned short pwm_min,
unsigned short pwm_max,
float angle_min,
float angle_max)
54 std::cout <<
"Synopsis" << std::endl
55 <<
" " << argv[0] <<
" [--device <name>] [--channel <number>] [--calibrate] [--range-pwm <min max> ] [--verbose, -v] [--help, -h]" << std::endl
57 std::cout <<
"Description" << std::endl
58 <<
" --device <name> Device name." << std::endl
59 <<
" Default: " << device << std::endl
61 <<
" --baud <rate> Serial link baud rate." << std::endl
62 <<
" Default: " << baudrate << std::endl
64 <<
" --channel <number> Channel to dial with." << std::endl
65 <<
" Default: " << channel << std::endl
67 <<
" --range-pwm <min max> Set PWM min and max values." << std::endl
68 <<
" You can use \"--calibrate\" to retrieve min and max pwm values."
69 <<
" Default: " << pwm_min <<
" " << pwm_max << std::endl
71 <<
" --range-angles <min max> Set angle min and max values (deg)." << std::endl
74 <<
" --verbose, -v Enable verbosity." << std::endl
76 <<
" --calibrate Start pwm calibration determining min and max admissible values." << std::endl
77 <<
" Once calibration done you can use \"--range-pwm <min max>\" option to set" << std::endl
78 <<
" the corresponding values" << std::endl
80 <<
" --help, -h Print this helper message." << std::endl
83 std::cout <<
"Error" << std::endl
85 <<
"Unsupported parameter " << argv[error] << std::endl;
89 int main(
int argc,
const char **argv)
92 std::string opt_device =
"COM4";
94 std::string opt_device =
"/dev/ttyACM0";
99 int opt_baudrate = 38400;
100 bool opt_verbose =
false;
101 bool opt_calibrate =
false;
102 unsigned short opt_pwm_min = 4000;
103 unsigned short opt_pwm_max = 8000;
104 float opt_angle_min =
static_cast<float>(
vpMath::rad(-45));
105 float opt_angle_max =
static_cast<float>(
vpMath::rad(45));
106 float opt_velocity =
static_cast<float>(
vpMath::rad(5));
107 float last_angle = 0;
109 for (
int i = 1; i < argc; i++) {
110 if (std::string(argv[i]) ==
"--device" && i + 1 < argc) {
111 opt_device = std::string(argv[i + 1]);
114 else if (std::string(argv[i]) ==
"--baud" && i + 1 < argc) {
115 opt_baudrate = std::atoi(argv[i + 1]);
118 else if (std::string(argv[i]) ==
"--channel" && i + 1 < argc) {
119 opt_channel = std::atoi(argv[i + 1]);
122 else if (std::string(argv[i]) ==
"--range-pwm" && i + 2 < argc) {
123 opt_pwm_min =
static_cast<unsigned short>(
vpMath::rad(std::atoi(argv[i + 1])));
124 opt_pwm_max =
static_cast<unsigned short>(
vpMath::rad(std::atoi(argv[i + 2])));
127 else if (std::string(argv[i]) ==
"--range-angles" && i + 2 < argc) {
128 opt_angle_min =
static_cast<float>(std::atof(argv[i + 1]));
129 opt_angle_max =
static_cast<float>(std::atof(argv[i + 2]));
132 else if (std::string(argv[i]) ==
"--calibrate") {
133 opt_calibrate =
true;
135 else if (std::string(argv[i]) ==
"--verbose" || std::string(argv[i]) ==
"-v") {
138 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
139 usage(argv, 0, opt_device, opt_baudrate, opt_channel, opt_pwm_min, opt_pwm_max, opt_angle_min, opt_angle_max);
143 usage(argv, i, opt_device, opt_baudrate, opt_channel, opt_pwm_min, opt_pwm_max, opt_angle_min, opt_angle_max);
148 std::chrono::seconds sec(1);
154 servo.connect(opt_device, opt_baudrate, opt_channel);
157 std::cout <<
"Proceed to calibration to determine pwm min and max values..." << std::endl;
158 std::cout <<
"WARNING: Calibration will move the servo at channel " << opt_channel <<
"!" << std::endl;
159 std::cout <<
"Press Enter to move to min and max pwm positions..." << std::endl;
162 unsigned short pwm_min, pwm_max;
163 servo.calibrate(pwm_min, pwm_max);
164 std::cout <<
"Servo on channel " << opt_channel <<
" has pwm range [" << pwm_min <<
", " << pwm_max <<
"]" << std::endl;
168 servo.setPwmRange(opt_pwm_min, opt_pwm_max);
169 servo.setAngularRange(opt_angle_min, opt_angle_max);
172 std::cout <<
"Move to zero position (deg): " <<
vpMath::deg(0) <<
" at max velocity" << std::endl;
173 servo.setAngularPosition(0, 0);
174 std::this_thread::sleep_for(std::chrono::seconds(3));
175 last_angle = servo.getAngularPosition();
176 std::cout <<
"Servo reached position (deg): " <<
vpMath::deg(last_angle) << std::endl;
181 std::cout <<
"Move at velocity (pwm): " << vel_pwm <<
" for 3 sec" << std::endl;
182 servo.setPwmVelocity(vel_pwm);
183 std::this_thread::sleep_for(3 * sec);
184 std::cout <<
"Servo reached position (pwm): " << servo.getPwmPosition() << std::endl;
187 std::cout <<
"Move at velocity (pwm): " << vel_pwm <<
" for 3 sec" << std::endl;
188 servo.setPwmVelocity(vel_pwm);
189 std::this_thread::sleep_for(3 * sec);
190 std::cout <<
"Servo reached position (pwm): " << servo.getPwmPosition() << std::endl;
191 std::cout <<
"End of velocity motion" << std::endl;
195 std::cout <<
"Move at velocity (deg/s): " <<
vpMath::deg(opt_velocity) <<
" for 3 sec" << std::endl;
196 servo.setAngularVelocity(opt_velocity);
197 std::this_thread::sleep_for(3 * sec);
198 std::cout <<
"Servo reached position (deg): " << servo.getPwmPosition() << std::endl;
200 std::cout <<
"Move at velocity (deg/s): " <<
vpMath::deg(-opt_velocity) <<
" for 3 sec" << std::endl;
201 servo.setAngularVelocity(-opt_velocity);
202 std::this_thread::sleep_for(3 * sec);
203 std::cout <<
"Servo reached position (deg): " << servo.getPwmPosition() << std::endl;
206 servo.stopVelocityCmd();
208 std::cout <<
"The end" << std::endl;
221 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.