YARP
Yet Another Robot Platform
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 true;
101}
102
104{
105 dd.close();
106
107 free(positions);
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 true;
131}
132
133
134bool SerialServoBoard::positionMove(int j, double ref)
135{
136 positions[j] = ref;
137
138 return move(j, ref, positions, speeds, serial);
139}
140
141
142bool SerialServoBoard::positionMove(const double* refs)
143{
144 for (int k = 0; k < 32; k++) {
145 this->positionMove(k, refs[k]);
146 }
147
148 return true;
149}
150
151
152bool SerialServoBoard::relativeMove(int j, double delta)
153{
154 this->positionMove(j, positions[j] + delta);
155
156 return true;
157}
158
159
160bool SerialServoBoard::relativeMove(const double* deltas)
161{
162 for (int k = 0; k < 32; k++) {
163 this->positionMove(k, positions[k] + deltas[k]);
164 }
165
166 return true;
167}
168
169
171{
172 //TODO: Q?
173
174 return true;
175}
176
177
179{
180 //TODO: Q?
181
182 return true;
183}
184
185
186bool SerialServoBoard::setRefSpeed(int j, double sp)
187{
188 speeds[j] = sp;
189
190 return true;
191}
192
193
194bool SerialServoBoard::setRefSpeeds(const double* spds)
195{
196 for (int k = 0; k < 32; k++) {
197 setRefSpeed(k, spds[k]);
198 }
199
200 return true;
201}
202
203
205{
206 return true;
207}
208
209
211{
212 return true;
213}
214
215
216bool SerialServoBoard::getRefSpeed(int j, double* ref)
217{
218 *ref = speeds[j];
219
220 return true;
221}
222
223
225{
226 for (int k = 0; k < 32; k++) {
227 spds[k] = speeds[k];
228 }
229
230 return true;
231}
232
233
235{
236 return true;
237}
238
239
241{
242 return true;
243}
244
245
247{
248 return true;
249}
250
251
253{
254 return true;
255}
256
257bool SerialServoBoard::positionMove(const int n_joint, const int* joints, const double* refs)
258{
259 return true;
260}
261bool SerialServoBoard::relativeMove(const int n_joint, const int* joints, const double* deltas)
262{
263 return true;
264}
265bool SerialServoBoard::checkMotionDone(const int n_joint, const int* joints, bool* flags)
266{
267 return true;
268}
269bool SerialServoBoard::setRefSpeeds(const int n_joint, const int* joints, const double* spds)
270{
271 return true;
272}
273bool SerialServoBoard::setRefAccelerations(const int n_joint, const int* joints, const double* accs)
274{
275 return true;
276}
277bool SerialServoBoard::getRefSpeeds(const int n_joint, const int* joints, double* spds)
278{
279 return true;
280}
281bool SerialServoBoard::getRefAccelerations(const int n_joint, const int* joints, double* accs)
282{
283 return true;
284}
285bool SerialServoBoard::stop(const int n_joint, const int* joints)
286{
287 return true;
288}
289
290
291bool movessc32(int j, double ref, double* positions, double* speeds, ISerialDevice* serial)
292{
293 int pos = 1500 + round(positions[j] * 11.11);
294
295 Bottle bot;
296 char str[80];
297 if (FABS(speeds[j]) < 0.1) {
298 std::snprintf(str, 80, "#%dP%d\r", j, pos);
299 } else {
300 int speed = round(speeds[j] * 11.11);
301
302 std::snprintf(str, 80, "#%dP%dS%d\r", j, pos, speed);
303 }
304 //if(j==0) {
305 bot.addString(str);
306 serial->send(bot);
307 //} else {
308 // serial->send(str, 2+strlen(str+3));
309 //}
310
311 return true;
312}
313
314
315bool moveminissc(int j, double ref, double* positions, double* speeds, ISerialDevice* serial)
316{
317 auto pos = (unsigned char)((int)(positions[j] * 1.411) + 127);
318
319 char cmd[3];
320
321 //ignore speed;
322 cmd[0] = 255; //sync byte
323 cmd[1] = (unsigned char)j; //servo number byte
324 cmd[2] = pos; //position byte
325
326 serial->send(cmd, 3);
327
328 return true;
329}
330
331
332bool movepontech(int j, double ref, double* positions, double* speeds, ISerialDevice* serial)
333{
334 auto pos = (unsigned char)((int)(positions[j] * 1.411) + 127);
335
336 Bottle bot;
337 char str[80];
338 std::snprintf(str, 80, "BD1SV%dM%d", j + 1, pos);
339
340 bot.addString(str);
341
342 serial->send(bot);
343
344 return true;
345}
346
347
348bool movemondotronic(int j, double ref, double* positions, double* speeds, ISerialDevice* serial)
349{
350 auto pos = (unsigned char)((int)(positions[j] * 1.411) + 127);
351
352 char cmd[3];
353
354 //ignore speed;
355 cmd[0] = 255; //sync byte
356 cmd[1] = (unsigned char)j; //servo number byte
357 cmd[2] = pos; //position byte (speed. this board controls speed for dc motors)
358
359 serial->send(cmd, 3);
360
361 return true;
362}
363
364
365bool movepololu(int j, double ref, double* positions, double* speeds, ISerialDevice* serial)
366{
367 int pos = 1500 + round(positions[j] * 11.11);
368
369 char cmd[6];
370
371 cmd[0] = 0x80;
372 cmd[1] = 0x01;
373
374 cmd[2] = 0x04;
375 cmd[3] = (unsigned char)j;
376
377 cmd[4] = (unsigned char)(pos >> 8); //high pos byte
378 cmd[5] = (unsigned char)pos; //low pos byte
379
380 serial->send(cmd, 6);
381
382 return true;
383}
384
385
386bool movepicopic(int j, double ref, double* positions, double* speeds, ISerialDevice* serial)
387{
388 int pos = 1500 + round(positions[j] * 11.11);
389
390 char cmd[5];
391
392 cmd[0] = (int)(j / 20) + 1; //board address
393 cmd[1] = (j + 1); //servo number (1-....)
394
395 cmd[2] = (unsigned char)(pos >> 8); //high pos byte
396 cmd[3] = (unsigned char)pos; //low pos byte
397
398 if (FABS(speeds[j]) < 0.1) {
399 cmd[4] = 255; //speed
400 } else {
401 auto speed = (unsigned char)((int)(speeds[j] * 1.411) + 127);
402
403 cmd[4] = speed; //speed
404 }
405
406 serial->send(cmd, 5);
407
408 return true;
409}
define control board standard interfaces
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)
#define MINISSC
#define SSC32
#define POLOLUUSB16
#define round(x)
#define FABS(x)
#define PONTECHSV203X
#define PICOPIC
#define MONDOTRONICSMI
bool getRefSpeed(int j, double *ref) override
Get reference speed for a joint.
bool(* move)(int j, double ref, double *positions, double *speeds, ISerialDevice *serial)
bool checkMotionDone(int j, bool *flag) override
Check if the current trajectory is terminated.
bool getRefAccelerations(double *accs) override
Get reference acceleration of all joints.
bool getRefAcceleration(int j, double *acc) override
Get reference acceleration for a joint.
bool getAxes(int *ax) override
Get the number of controlled axes.
ISerialDevice * serial
bool close() override
Close the DeviceDriver.
bool setRefSpeeds(const double *spds) override
Set reference speed on all joints.
bool getRefSpeeds(double *spds) override
Get reference speed of all joints.
bool setRefSpeed(int j, double sp) override
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
bool open(Searchable &config) override
Open the DeviceDriver.
bool stop() override
Stop motion, multiple joints.
bool setRefAcceleration(int j, double acc) override
Set reference acceleration for a joint.
bool positionMove(int j, double ref) override
Set new reference point for a single axis.
bool setRefAccelerations(const double *accs) override
Set reference acceleration on all joints.
bool relativeMove(int j, double delta) override
Set relative position.
bool view(T *&x)
Get an interface to the device driver.
Definition: DeviceDriver.h:88
A generic interface to serial port devices.
Definition: ISerialDevice.h:24
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.
Definition: PolyDriver.cpp:173
bool isValid() const
Check if device is valid.
Definition: PolyDriver.cpp:196
bool open(const std::string &txt)
Construct and configure a device by its common name.
Definition: PolyDriver.cpp:140
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:64
void addString(const char *str)
Places a string in the bottle, at the end of the list.
Definition: Bottle.cpp:170
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:1015
A base class for nested structures that can be searched.
Definition: Searchable.h:56
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:43
#define yCInfo(component,...)
Definition: LogComponent.h:171
#define yCError(component,...)
Definition: LogComponent.h:213
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:76
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.