111 #include <yarp/os/Network.h>
112 #include <yarp/os/Property.h>
113 #include <yarp/os/Time.h>
114 #include <yarp/dev/PolyDriver.h>
115 #include <yarp/os/BufferedPort.h>
116 #include <yarp/os/RFModule.h>
118 #include <yarp/os/Log.h>
119 #include <yarp/os/LogStream.h>
125 using namespace yarp::os;
139 virtual bool respond(
const yarp::os::Bottle &command, yarp::os::Bottle &reply)
142 yDebug(
"receiving command from port\n");
144 int cmdSize = command.size();
148 switch(command.get(index).asVocab32())
150 case yarp::os::createVocab32(
's',
'u',
's',
'p'):
151 reply.addVocab32(
"ack");
156 case yarp::os::createVocab32(
'r',
'u',
'n'):
157 reply.addVocab32(
"ack");
163 case yarp::os::createVocab32(
's',
'e',
't'):
164 if (command.size()>=3)
166 int i=command.get(index+1).asInt32();
167 double pos=command.get(index+2).asFloat64();
176 yError(
"Invalid set message, ignoring\n");
178 reply.addVocab32(
"ack");
181 case yarp::os::createVocab32(
's',
'v',
'e',
'l'):
182 if(command.size()>=3)
184 int i=command.get(index+1).asInt32();
185 double vel = command.get(index+2).asFloat64();
189 reply.addVocab32(
"ack");
195 yError(
"Invalid set vel message, ignoring\n");
196 reply.addVocab32(
"fail");
199 case yarp::os::createVocab32(
'g',
'a',
'i',
'n'):
200 if(command.size()>=3)
202 int i=command.get(index+1).asInt32();
203 double gain = command.get(index+2).asFloat64();
207 reply.addVocab32(
"ack");
213 yError(
"Invalid set gain message, ignoring\n");
214 reply.addVocab32(
"fail");
217 case yarp::os::createVocab32(
'h',
'e',
'l',
'p'):
218 fprintf(stdout,
"VelocityControl module, valid commands are:\n");
219 fprintf(stdout,
"- [susp] suspend the controller (command zero velocity)\n");
220 fprintf(stdout,
"- [run] start (and resume after being suspended) the controller\n");
221 fprintf(stdout,
"- [quit] quit the module (exit)\n");
222 fprintf(stdout,
"- [set] <j> <p> move joint j to p (degrees)\n");
223 fprintf(stdout,
"- [svel] <j> <v> set maximum speed for joint j to v (deg/sec)\n");
224 fprintf(stdout,
"- [gain] <j> <k> set P gain for joint j to k\n");
225 fprintf(stdout,
"- [help] to get this help\n");
226 fprintf(stdout,
"\n typical commands:\n gain 0 10\n svel 0 10\n run\n set 0 x\n\n");
228 reply.addVocab32(
"many");
229 reply.addVocab32(
"ack");
230 reply.addString(
"VelocityControl module, valid commands are:");
231 reply.addString(
"- [susp] suspend the controller (command zero velocity)");
232 reply.addString(
"- [run] start (and resume after being suspended) the controller");
233 reply.addString(
"- [quit] quit the module (exit)");
234 reply.addString(
"- [set] <j> <p> move joint j to p (degrees)");
235 reply.addString(
"- [svel] <j> <v> set maximum speed for joint j to v (deg/sec)");
236 reply.addString(
"- [gain] <j> <k> set P gain for joint j to k");
237 reply.addString(
"- [help] to get this help");
238 reply.addString(
"\n typical commands:\n gain 0 10\n svel 0 10\n run\n set 0 x\n\n");
241 yError(
"Invalid command, ignoring\n");
242 reply.addVocab32(
"fail");
257 options.fromString(rf.toString());
259 options.put(
"device",
"remote_controlboard");
260 if(options.check(
"robot"))
261 strncpy(robotName, options.find(
"robot").asString().c_str(),
sizeof(robotName));
263 strncpy(robotName,
"icub",
sizeof(robotName));
265 if(options.check(
"part"))
268 snprintf(
tmp,
sizeof(
tmp),
"/%s/vc/%s/client",
270 options.find(
"part").asString().c_str());
271 options.put(
"local",
tmp);
273 snprintf(
tmp,
sizeof(
tmp),
"/%s/%s",
275 options.find(
"part").asString().c_str());
276 options.put(
"remote",
tmp);
278 snprintf(
tmp,
sizeof(
tmp),
"/%s/vc/%s/input",
280 options.find(
"part").asString().c_str());
281 input_port.open(
tmp);
283 options.put(
"carrier",
"mcast");
289 yError(
"Please specify part (e.g. --part head)\n");
294 if (!driver.open(options))
296 yError(
"Error opening device, check parameters\n");
302 if(options.check(
"period"))
303 period = options.find(
"period").asInt32();
305 yInfo(
"control rate is %d ms",period);
307 if (!options.check(
"part"))
310 snprintf(partName,
sizeof(partName),
"%s", options.find(
"part").asString().c_str());
313 vc->
init(&driver, partName,
322 yInfo(
"Closing module [%s]\n", partName);
326 yInfo(
"Thead [%s] stopped\n", partName);
331 yInfo(
"Module [%s] closed\n", partName);
351 yInfo(
"Main returning\n");
virtual bool configure(yarp::os::ResourceFinder &rf)
virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
void setGain(int i, double gain)
void setRef(int i, double pos)
bool init(yarp::dev::PolyDriver *d, std::string partName, std::string robotName)
void setVel(int i, double vel)
int main(int argc, char *argv[])
Copyright (C) 2008 RobotCub Consortium.