Table of Contents
Let’s write a UObject for a servomotor device whose underlying API is:
bool initialize (int id); //initialize the servomotor with given ID
double getPosition (int id); //read servomotor of given id position
void setPosition (int id, double pos); //send a command to servomotor
void setPID (int id, int p, int i, int d); //set PID arguments
First our header. Our servo device provides an attribute named "val", the standard URBI name, and two ways to set PID gain: a method, and three variables.
class servo : public urbi::UObject //must inherits UObject
{
public:
// the class must have a single constructor taking a string
servo(const std::string&);
// URBI constructor
int init(int id);
// main attribute
urbi::UVar val;
// position variables:
// P gain
urbi::UVar P;
// I gain
urbi::UVar I;
// D gain
urbi::UVar D;
// callback for val change
int valueChanged(UVar &v);
//callback for val access
int valueAccessed(UVar &v);
// callback for P I D change
int pidChanged(UVar &v);
// method to change all values
void setPID(int p, int i, int d);
// motor ID
int id_;
};
The constructor only registers init, so that our default instance "servo" does nothing, and can only be used to create new instances.
servo::servo (const std::string& s)
: urbi::UObject (s)
{
// register init
UBindFunction (servo, init);
}
The init function, called in a new instance each time a new URBI instance is created, registers the three variables (val, P, I and D), and sets up callback functions.
// URBI constructor
int
servo::init (int id)
{
id_ = id;
if (!initialize (id))
return 1;
UBindVar (servo, val);
// val is both a sensor and an actuator
Uowned (val);
// set blend mode to mix
val.blend = urbi::UMIX;
// register variables
UBindVar (servo, P);
UBindVar (servo, I);
UBindVar (servo, D);
// register functions
UBindFunction (servo, setPID);
// register callbacks on functions
UNotifyChange (val, &servo::valueChanged);
UNotifyAccess (val, &servo::valueAccessed);
UNotifyChange (P, &servo::pidChanged);
UNotifyChange (I, &servo::pidChanged);
UNotifyChange (D, &servo::pidChanged);
return 0;
}
Then we define our callback methods. servo::valueChanged will be called each time the val variable is modified, just after the value is changed: we use this method to send our servo command. servo::valueAccessed is called just before the value is going to be read. In this function we request the current value from the servo, and set val accordingly.
//called each time val is written to
int
servo::valueChanged (urbi::UVar & v)
{
// v is a ref. to val
setPosition (id, (double)val);
return 0;
}
// called each time val is read
int
servo::valueAccessed(urbi::UVar & v)
{
// v is a ref. to val
val = getPosition (id);
return 0;
}
servo::pidChanged is called each time one of the PID variables is written to. The function servo::setPID can be called directly from URBI.
int
servo::pidChanged (urbi::UVar &v)
{
setPID(id, (int)P, (int)I, (int)D);
return 1;
}
void
servo::setPID (int p, int i, int d)
{
setPID (id, p, i, d);
P = p;
I = i;
D = d;
}
//register servo class to the URBI kernel
UStart (servo);
That’s it, compile this module, and you can use it within URBI:
//creates a new instance, and calls init (1) headPan = new servo (1); //calls setPID () headPan.setPID (8,2,1); // calls valueChanged () headPan.val=13; // calls valueAccessed () headPan.val * 12; // periodically calls valueChanged () headPan.val = 0 sin:1s ampli:20, // periodically calls valueAccessed () at (headPan.val < 0) echo "left";
The sample code above has one problem: valueAccessed and valueChanged are called each time the value is read or written from URBI, which can happen quite often. This is a problem if sending the actual command (setPosition in our example) takes time to execute. There are two solutions to this issue:
One solution is to remember the last time the value was read/written, and not apply the new command before a fixed time. Note that the kernel is doing this automatically for Uowned()’d variables that are in a blend mode different than "normal". So the easiest solution to the above problem is likely to set the variable to the "mix" blending mode. The unavoidable drawback is that commands are not applied immediately, but only after a small delay.
Instead of updating/fetching the value on demand, you can chose to do it periodically based on a timer. A small difference between the two API methods comes in handy for this case: the update () virtual method called periodically after being set up by USetUpdate (interval) is called just after one pass of URBI code execution, whereas the timers set up by USetTimer are called just before one pass of URBI code execution. So the ideal solution is to read your sensors in the second callback, and write to your actuators in the first. Our previous example (ommiting PID handling for clarity) can be rewritten. The header becomes:
// inherits from UObject
class servo : public urbi::UObject
{
public:
//the class must have a single constructor taking a string
servo (const std::string&)
// URBI constructor
int init (int id);
// called periodically
virtual int update ();
// called periodically
int getVal ();
// our position variable
urbi::UVar val;
// motor ID
int id_;
};
Constructor is unchanged, init becomes:
int
servo::init (int id)
{
// urbi constructor
id_ = id;
if (!initialize (id))
return 0;
UBindVar (servo,val);
//val is both a sensor and an actuator
UOwned(val);
// will call update () periodically
USetUpdate(1);
// idem for getVal ()
USetTimer (1, &servo::getVal);
return 0;
}
valueChanged becomes update and valueAccessed becomes getVal. Instead of being called on demand, they are now called periodically. The period of the call cannot be lower than the server period (which is chosen when starting the robot, or fixed by the underlying architecture), so you can set it to 0 to mean "as fast as is usefull".