YARP
Yet Another Robot Platform
dev/fake_motor.cpp

Some tips on how to create a device for a new motor control board.

/*
* Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
* Copyright (C) 2006-2010 RobotCub Consortium
* All rights reserved.
*
* This software may be modified and distributed under the terms of the
* BSD-3-Clause license. See the accompanying LICENSE file for details.
*/
#include <yarp/os/all.h>
#include <stdio.h>
using namespace yarp::os;
using namespace yarp::dev;
class FakeMotor : public DeviceDriver, public IPositionControl {
public:
virtual bool getAxes(int *ax) {
*ax = 2;
printf("FakeMotor reporting %d axes present\n", *ax);
return true;
}
virtual bool positionMove(int j, double ref) {
return true;
}
virtual bool positionMove(const double *refs) {
return true;
}
virtual bool relativeMove(int j, double delta) {
return true;
}
virtual bool relativeMove(const double *deltas) {
return true;
}
virtual bool checkMotionDone(int j, bool *flag) {
return true;
}
virtual bool checkMotionDone(bool *flag) {
return true;
}
virtual bool setRefSpeed(int j, double sp) {
return true;
}
virtual bool setRefSpeeds(const double *spds) {
return true;
}
virtual bool setRefAcceleration(int j, double acc) {
return true;
}
virtual bool setRefAccelerations(const double *accs) {
return true;
}
virtual bool getRefSpeed(int j, double *ref) {
return true;
}
virtual bool getRefSpeeds(double *spds) {
return true;
}
virtual bool getRefAcceleration(int j, double *acc) {
return true;
}
virtual bool getRefAccelerations(double *accs) {
return true;
}
virtual bool stop(int j) {
return true;
}
virtual bool stop() {
return true;
}
virtual bool open(Searchable& config) {
return true;
}
virtual bool close() {
return true;
}
};
void testMotor(PolyDriver& driver) {
if (driver.view(pos)) {
int ct = 0;
pos->getAxes(&ct);
printf(" number of axes is: %d\n", ct);
} else {
printf(" could not find IPositionControl interface\n");
}
}
int main(int argc, char *argv[]) {
Drivers::factory().add(new DriverCreatorOf<FakeMotor>("motor",
"controlboard",
"FakeMotor"));
printf("============================================================\n");
printf("check our device can be instantiated directly\n");
PolyDriver direct("motor");
if (direct.isValid()) {
printf("Direct instantiation worked\n");
testMotor(direct);
} else {
printf("Direct instantiation failed\n");
}
direct.close();
// check our device can be wrapped in the controlboard network wrapper
printf("\n\n");
printf("============================================================\n");
printf("check our device can be wrapped in controlboard\n");
PolyDriver indirect("(device controlboard) (subdevice motor)");
if (indirect.isValid()) {
printf("Indirect instantiation worked\n");
} else {
printf("Indirect instantiation failed\n");
}
indirect.close();
// check our device can be wrapped in the controlboard network wrapper
// and accessed remotely
printf("\n\n");
printf("============================================================\n");
printf("check our device can be accessed via remote_controlboard\n");
PolyDriver server("(device controlboard) (subdevice motor) (name /server)");
if (server.isValid()) {
printf("Server instantiation worked\n");
PolyDriver client("(device clientcontrolboard) (local /client) (remote /server)");
if (client.isValid()) {
printf("Client instantiation worked\n");
testMotor(client);
} else {
printf("Client instantiation failed\n");
}
client.close();
}
server.close();
return 0;
}