YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
SerialServoBoard.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-FileCopyrightText: 2008 Giacomo Spigler
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7#include "SerialServoBoard.h"
8
10
12#include <yarp/dev/Drivers.h>
14#include <yarp/dev/PolyDriver.h>
15
16#include <cstdio>
17#include <cstdlib>
18
19
20//TODO: check limits of operation (range of angles)?
21
22
23using namespace yarp::os;
24using namespace yarp::dev;
25
26namespace {
27YARP_LOG_COMPONENT(SERIALSERVOBOARD, "yarp.devices.SerialServoBoard")
28}
29
30
32{
33 if (config.check("help") == true) {
34 yCInfo(SERIALSERVOBOARD, "SerialServoBoard Available Options:");
35 yCInfo(SERIALSERVOBOARD, " -board NAME, where name is one of ssc32, minissc, pontech_sv203x, mondotronic_smi, parallax, pololu_usb_16servo, picopic");
36 yCInfo(SERIALSERVOBOARD, " -comport NAME, where name is COMx on Windows, and /dev/ttySx on Linux");
37 yCInfo(SERIALSERVOBOARD, " -baudrate RATE, where RATE is the Board baudrate");
38 yCInfo(SERIALSERVOBOARD, " -help shows this help");
39
40 return false;
41 }
42
43
44 char servoboard_[80];
45
46 strcpy(servoboard_, config.check("board", yarp::os::Value("ssc32")).asString().c_str());
47
48 if (strcmp(servoboard_, "ssc32") == 0) {
50 move = &movessc32;
51 } else if (strcmp(servoboard_, "minissc") == 0) {
54 } else if (strcmp(servoboard_, "pontech_sv203x") == 0) {
57 } else if (strcmp(servoboard_, "mondotronic_smi") == 0) {
60 } else if (strcmp(servoboard_, "pololu_usb_16servo") == 0) {
63 } else if (strcmp(servoboard_, "picopic") == 0) {
66 }
67
68
69 char comport[80];
70
71 strcpy(comport, config.check("comport", yarp::os::Value("/dev/ttyS0")).asString().c_str());
72
73 int baudrate = config.check("baudrate", yarp::os::Value(38400)).asInt32();
74
75 Property conf;
76 // no arguments, use a default
77 conf.put("device", "serialport");
78 conf.put("comport", comport);
79 conf.put("baudrate", baudrate);
80 conf.put("rcvenb", 1);
81 conf.put("stopbits", 2);
82 conf.put("databits", 8);
83 conf.put("paritymode", "none");
84
85 dd.open(conf);
86 if (!dd.isValid()) {
87 yCError(SERIALSERVOBOARD, "Failed to create and configure a device");
88 std::exit(1);
89 }
90
91 if (!dd.view(serial)) {
92 yCError(SERIALSERVOBOARD, "Failed to view device through ISerialDevice interface");
93 std::exit(1);
94 }
95
96
97 positions = (double*)malloc(sizeof(double) * 32);
98 speeds = (double*)malloc(sizeof(double) * 32);
99
100 return ReturnValue_ok;
101}
102
104{
105 dd.close();
106
108 free(speeds);
109
110 return true;
111}
112
113
115{
116 if (servoboard == SSC32) {
117 ax = 32;
118 } else if (servoboard == MINISSC) {
119 ax = 8;
120 } else if (servoboard == PONTECHSV203X) {
121 ax = 8;
122 } else if (servoboard == MONDOTRONICSMI) {
123 ax = 2;
124 } else if (servoboard == POLOLUUSB16) {
125 ax = 16;
126 } else if (servoboard == PICOPIC) {
127 ax = 20;
128 }
129
130 return ReturnValue_ok;
131}
132
133
135{
136 positions[j] = ref;
137
138 if (move(j, ref, positions, speeds, serial))
139 return ReturnValue_ok;
140 return ReturnValue::return_code::return_value_error_method_failed;
141}
142
143
145{
146 for (int k = 0; k < 32; k++) {
147 this->positionMove(k, refs[k]);
148 }
149
150 return ReturnValue_ok;
151}
152
153
155{
156 this->positionMove(j, positions[j] + delta);
157
158 return ReturnValue_ok;
159}
160
161
163{
164 for (int k = 0; k < 32; k++) {
165 this->positionMove(k, positions[k] + deltas[k]);
166 }
167
168 return ReturnValue_ok;
169}
170
171
173{
174 //TODO: Q?
175
176 return ReturnValue_ok;
177}
178
179
181{
182 //TODO: Q?
183
184 return ReturnValue_ok;
185}
186
187
189{
190 speeds[j] = sp;
191
192 return ReturnValue_ok;
193}
194
195
197{
198 for (int k = 0; k < 32; k++) {
199 setTrajSpeed(k, spds[k]);
200 }
201
202 return ReturnValue_ok;
203}
204
205
207{
208 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
209}
210
211
213{
214 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
215}
216
217
219{
220 *ref = speeds[j];
221
222 return ReturnValue_ok;
223}
224
225
227{
228 for (int k = 0; k < 32; k++) {
229 spds[k] = speeds[k];
230 }
231
232 return ReturnValue_ok;
233}
234
235
237{
238 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
239}
240
241
243{
244 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
245}
246
247
249{
250 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
251}
252
253
255{
256 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
257}
258
259ReturnValue SerialServoBoard::positionMove(const int n_joint, const int* joints, const double* refs)
260{
261 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
262}
263ReturnValue SerialServoBoard::relativeMove(const int n_joint, const int* joints, const double* deltas)
264{
265 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
266}
267ReturnValue SerialServoBoard::checkMotionDone(const std::vector<int>& joints, bool& flags)
268{
269 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
270}
271ReturnValue SerialServoBoard::setTrajSpeeds(const int n_joint, const int* joints, const double* spds)
272{
273 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
274}
275ReturnValue SerialServoBoard::setTrajAccelerations(const int n_joint, const int* joints, const double* accs)
276{
277 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
278}
279ReturnValue SerialServoBoard::getTrajSpeeds(const int n_joint, const int* joints, double* spds)
280{
281 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
282}
283ReturnValue SerialServoBoard::getTrajAccelerations(const int n_joint, const int* joints, double* accs)
284{
285 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
286}
287ReturnValue SerialServoBoard::stop(const int n_joint, const int* joints)
288{
289 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
290}
291
293{
294 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
295}
296
298{
299 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
300}
301
303{
304 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
305}
306
307bool movessc32(int j, double ref, double* positions, double* speeds, ISerialDevice* serial)
308{
309 int pos = 1500 + round(positions[j] * 11.11);
310
311 Bottle bot;
312 char str[80];
313 if (FABS(speeds[j]) < 0.1) {
314 std::snprintf(str, 80, "#%dP%d\r", j, pos);
315 } else {
316 int speed = round(speeds[j] * 11.11);
317
318 std::snprintf(str, 80, "#%dP%dS%d\r", j, pos, speed);
319 }
320 //if(j==0) {
321 bot.addString(str);
322 serial->send(bot);
323 //} else {
324 // serial->send(str, 2+strlen(str+3));
325 //}
326
327 return ReturnValue_ok;
328}
329
330
331bool moveminissc(int j, double ref, double* positions, double* speeds, ISerialDevice* serial)
332{
333 auto pos = (unsigned char)((int)(positions[j] * 1.411) + 127);
334
335 char cmd[3];
336
337 //ignore speed;
338 cmd[0] = 255; //sync byte
339 cmd[1] = (unsigned char)j; //servo number byte
340 cmd[2] = pos; //position byte
341
342 serial->send(cmd, 3);
343
344 return ReturnValue_ok;
345}
346
347
348bool movepontech(int j, double ref, double* positions, double* speeds, ISerialDevice* serial)
349{
350 auto pos = (unsigned char)((int)(positions[j] * 1.411) + 127);
351
352 Bottle bot;
353 char str[80];
354 std::snprintf(str, 80, "BD1SV%dM%d", j + 1, pos);
355
356 bot.addString(str);
357
358 serial->send(bot);
359
360 return ReturnValue_ok;
361}
362
363
364bool movemondotronic(int j, double ref, double* positions, double* speeds, ISerialDevice* serial)
365{
366 auto pos = (unsigned char)((int)(positions[j] * 1.411) + 127);
367
368 char cmd[3];
369
370 //ignore speed;
371 cmd[0] = 255; //sync byte
372 cmd[1] = (unsigned char)j; //servo number byte
373 cmd[2] = pos; //position byte (speed. this board controls speed for dc motors)
374
375 serial->send(cmd, 3);
376
377 return ReturnValue_ok;
378}
379
380
381bool movepololu(int j, double ref, double* positions, double* speeds, ISerialDevice* serial)
382{
383 int pos = 1500 + round(positions[j] * 11.11);
384
385 char cmd[6];
386
387 cmd[0] = 0x80;
388 cmd[1] = 0x01;
389
390 cmd[2] = 0x04;
391 cmd[3] = (unsigned char)j;
392
393 cmd[4] = (unsigned char)(pos >> 8); //high pos byte
394 cmd[5] = (unsigned char)pos; //low pos byte
395
396 serial->send(cmd, 6);
397
398 return ReturnValue_ok;
399}
400
401
402bool movepicopic(int j, double ref, double* positions, double* speeds, ISerialDevice* serial)
403{
404 int pos = 1500 + round(positions[j] * 11.11);
405
406 char cmd[5];
407
408 cmd[0] = (int)(j / 20) + 1; //board address
409 cmd[1] = (j + 1); //servo number (1-....)
410
411 cmd[2] = (unsigned char)(pos >> 8); //high pos byte
412 cmd[3] = (unsigned char)pos; //low pos byte
413
414 if (FABS(speeds[j]) < 0.1) {
415 cmd[4] = 255; //speed
416 } else {
417 auto speed = (unsigned char)((int)(speeds[j] * 1.411) + 127);
418
419 cmd[4] = speed; //speed
420 }
421
422 serial->send(cmd, 5);
423
424 return ReturnValue_ok;
425}
define control board standard interfaces
#define ReturnValue_ok
Definition ReturnValue.h:82
bool movemondotronic(int j, double ref, double *positions, double *speeds, ISerialDevice *serial)
bool movepontech(int j, double ref, double *positions, double *speeds, ISerialDevice *serial)
bool movepicopic(int j, double ref, double *positions, double *speeds, ISerialDevice *serial)
bool movepololu(int j, double ref, double *positions, double *speeds, ISerialDevice *serial)
bool movessc32(int j, double ref, double *positions, double *speeds, ISerialDevice *serial)
bool moveminissc(int j, double ref, double *positions, double *speeds, ISerialDevice *serial)
bool movemondotronic(int j, double ref, double *positions, double *speeds, ISerialDevice *serial)
bool movepontech(int j, double ref, double *positions, double *speeds, ISerialDevice *serial)
#define MINISSC
#define SSC32
#define POLOLUUSB16
bool movepicopic(int j, double ref, double *positions, double *speeds, ISerialDevice *serial)
#define round(x)
bool movepololu(int j, double ref, double *positions, double *speeds, ISerialDevice *serial)
#define FABS(x)
bool movessc32(int j, double ref, double *positions, double *speeds, ISerialDevice *serial)
bool moveminissc(int j, double ref, double *positions, double *speeds, ISerialDevice *serial)
#define PONTECHSV203X
#define PICOPIC
#define MONDOTRONICSMI
yarp::dev::ReturnValue setTrajAccelerations(const double *accs) override
Set reference acceleration on all joints.
bool(* move)(int j, double ref, double *positions, double *speeds, ISerialDevice *serial)
yarp::dev::ReturnValue getTrajSpeed(int j, double *ref) override
Get reference speed for a joint.
yarp::dev::ReturnValue stop() override
Stop motion, multiple joints.
yarp::dev::ReturnValue getTrajSpeeds(double *spds) override
Get reference speed of all joints.
yarp::dev::ReturnValue positionMove(int j, double ref) override
Set new reference point for a single axis.
yarp::dev::ReturnValue relativeMove(int j, double delta) override
Set relative position.
yarp::dev::ReturnValue setTrajSpeeds(const double *spds) override
Set reference speed on all joints.
ISerialDevice * serial
bool close() override
Close the DeviceDriver.
yarp::dev::ReturnValue getTrajAcceleration(int j, double *acc) override
Get reference acceleration for a joint.
yarp::dev::ReturnValue setTrajSpeed(int j, double sp) override
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
yarp::dev::ReturnValue getAxes(size_t &ax) override
Get the number of controlled axes.
yarp::dev::ReturnValue checkMotionDone(int j, bool &flag) override
Check if the current trajectory is terminated.
yarp::dev::ReturnValue getTrajAccelerations(double *accs) override
Get reference acceleration of all joints.
yarp::dev::ReturnValue getTargetPositions(double *)
Get the last position reference for all axes.
yarp::dev::ReturnValue setTrajAcceleration(int j, double acc) override
Set reference acceleration for a joint.
bool open(Searchable &config) override
Open the DeviceDriver.
yarp::dev::ReturnValue getTargetPosition(const int, double *)
Get the last position reference for the specified axis.
bool view(T *&x)
Get an interface to the device driver.
A generic interface to serial port devices.
virtual bool send(const yarp::os::Bottle &msg)=0
Sends a string of chars to the serial communications channel.
bool close() override
Close the DeviceDriver.
bool isValid() const
Check if device is valid.
bool open(const std::string &txt)
Construct and configure a device by its common name.
A simple collection of objects that can be described and transmitted in a portable way.
Definition Bottle.h:65
void addString(const char *str)
Places a string in the bottle, at the end of the list.
Definition Bottle.cpp:176
A mini-server for performing network communication in the background.
A class for storing options and configuration information.
Definition Property.h:33
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
Definition Property.cpp:987
A base class for nested structures that can be searched.
Definition Searchable.h:31
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
A single value (typically within a Bottle).
Definition Value.h:44
#define yCInfo(component,...)
#define yCError(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.
Definition OSTestMsg.cpp:13