void BDManager::addRequest( BDRequest* bdr )
{
if( bdr->getDevID() < getNumberOfDevices() )
getDeviceByNumber( bdr->getDevID() )->addRequest( bdr );
else
bdr->setStatus( BDRequest::BD_ERROR );
}
AudioOutput::AudioOutput()
{
try
{
if (getNumberOfDevices())
{
id_ = audio_.getDefaultOutputDevice();
open(id_);
}
api_ = audio_.getCurrentApi();
}
catch(RtAudioError& error)
{
error.printMessage();
}
apiName_ = getApiName(api_);
}
bool yarp::dev::UrbtcControl::open(yarp::os::Searchable& config){
// ConstString fname =
// config.check("source",
// Value("default.avi"), // a default value
// "movie file to read from").asString();
//cout << "Urbtc config: " << config.toString() << endl;
// checking for number of attached devices
_intNumControllers = getNumberOfDevices();
if (_intNumControllers == 0){
printf("No controller device files found. Controllers attached? Access rights ok?\n");
return false;
}
// init data structures
setDeviceFilenames(_intNumControllers);
_fd = new int[_intNumControllers];
_cmd = new ccmd[_intNumControllers];
_out = new uout[_intNumControllers];
_in = new uin[_intNumControllers];
_pos = new long[_intNumControllers];
_posOld = new long[_intNumControllers];
_posDesired = new long[_intNumControllers];
_periodCounter = new int[_intNumControllers];
for (int i = 0; i < _intNumControllers; i++){
_pos[i] = 0;
_posOld[i] = 0;
_posDesired[i] = 0;
_periodCounter[i] = 0;
}
bool configValid = true;
// encoder tic factors
double *encoderFactors = new double[_intNumControllers];
if (config.check("encoderFactors", "how many encoder tics are 1 radian/degree? List of double values (1 value for each axis)")){
Bottle botEncoderFactors = config.findGroup("encoderFactors");
if (botEncoderFactors.size() > _intNumControllers){ // bottle contains key as well
for (int i = 1; i <= _intNumControllers; i++)
encoderFactors[i-1] = botEncoderFactors.get(i).asDouble();
}
else{
configValid = false;
printf("Caution: UrbtcControl::open(): encoderFactors set to default, number of encoder factors in configuration too low.\n");
}
}
else{
configValid = false;
}
if (!configValid){
// default configuration
for (int i = 0; i < _intNumControllers; i++)
encoderFactors[i] = _dblFactDeg2Raw;
}
configValid = true;
// axis map
int *axisMap = new int[_intNumControllers];
if (config.check("axisMap","Mapping of axis indices. List of int values (1 value for each axis)")){
Bottle botAxisMap = config.findGroup("axisMap");
if (botAxisMap.size() > _intNumControllers){ // bottle contains key as well
for (int i = 1; i <= _intNumControllers; i++)
axisMap[i-1] = botAxisMap.get(i).asInt();
}
else{
configValid = false;
printf("Caution: UrbtcControl::open(): axisMap set to default, number of values in configuration too low.\n");
}
}
else{
configValid = false;
printf("Caution: UrbtcControl::open(): axisMap set to default, values not found in configuration.\n");
}
if (!configValid){
// default configuration
for (int i = 0; i < _intNumControllers; i++)
axisMap[i] = i;
}
configValid = true;
// limits
_limitsMax = new int[_intNumControllers];
_limitsMin = new int[_intNumControllers];
config.check("limitsMax", "+ limits in radian. List of double values (1 for each axis)."); // for --verbose switch only
config.check("limitsMin","- limits in radian, List of double values (1 for each axis)."); // for --verbose switch only
if (config.check("limitsMax") &&
config.check("limitsMin")){
Bottle botLimitsMin = config.findGroup("limitsMin");
Bottle botLimitsMax = config.findGroup("limitsMax");
if (botLimitsMin.size() > _intNumControllers && botLimitsMax.size() > _intNumControllers){ // bottle contains key as well
for (int i = 1; i <= _intNumControllers; i++){
_limitsMin[i-1] = (int)(botLimitsMin.get(i).asDouble() * encoderFactors[i-1]);
_limitsMax[i-1] = (int)(botLimitsMax.get(i).asDouble() * encoderFactors[i-1]);
}
}
else{
configValid = false;
printf("Caution: UrbtcControl::open(): limitsMin, limitsMax set to default, number of values in configuration too low.\n");
}
}
else{
configValid = false;
printf("Caution: UrbtcControl::open(): limitsMin, limitsMax set to default, values not found in configuration.\n");
}
if (!configValid){
// default configuration
for (int i = 0; i < _intNumControllers; i++){
_limitsMin[i] = -_intLimitDefault;
_limitsMax[i] = _intLimitDefault;
}
}
configValid = true;
// pid values
config.check("kp", "The p values for the PID controller. List of double values (1 for each axis).");
config.check("ki", "The i values for the PID controller. List of double values (1 for each axis).");
config.check("kd", "The d values for the PID controller. List of double values (1 for each axis).");
if (config.check("kp") &&
config.check("ki") &&
config.check("kd")){
Bottle botKp = config.findGroup("kp");
Bottle botKi = config.findGroup("ki");
Bottle botKd = config.findGroup("kd");
if (botKp.size() > _intNumControllers && botKi.size() > _intNumControllers && botKd.size() > _intNumControllers){ // bottle contains key as well
for (int i = 1; i <= _intNumControllers; i++){
for (int j=0; j<4; j++) {
_out[i-1].ch[j].x = 0; // target position
_out[i-1].ch[j].d = 0; // target velocity
_out[i-1].ch[j].kp = (signed short)(botKp.get(i).asDouble() * _pidAccuracy);
_out[i-1].ch[j].kpx = (unsigned short) _pidAccuracy; // P gain = 5/1 = 5
_out[i-1].ch[j].kd = (signed short)(botKd.get(i).asDouble() * _pidAccuracy);
_out[i-1].ch[j].kdx =(unsigned short) _pidAccuracy; // D gain = 2/1 = 2
_out[i-1].ch[j].ki = (signed short)(botKi.get(i).asDouble() * _pidAccuracy);
_out[i-1].ch[j].kix = (unsigned short)_pidAccuracy; // I gain = 0/1 = 0
//printf ("kp nomin: %lf\n" , (botKp.get(i).asDouble() * _pidAccuracy));
//printf ("kd nomin: %lf\n" , (botKd.get(i).asDouble() * _pidAccuracy));
//printf ("ki nomin: %lf\n" , (botKi.get(i).asDouble() * _pidAccuracy));
}
}
}
else{
configValid = false;
printf("Caution: UrbtcControl::open(): kp, ki, kd set to default, number of values in configuration too low.\n");
}
}
else{
configValid = false;
printf("Caution: UrbtcControl::open(): kp, ki, kd set to default, values not found in configuration.\n");
}
if (!configValid){
// default configuration
for (int i = 0; i < _intNumControllers; i++){
for (int j=0; j<4; j++) {
_out[i].ch[j].x = 0; // target position
_out[i].ch[j].d = 0; // target velocity
_out[i].ch[j].kp = (signed short)(_pidPosDefault.kp * _pidAccuracy);
_out[i].ch[j].kpx = (unsigned short) _pidAccuracy; // P gain = 5/1 = 5
_out[i].ch[j].kd = (signed short)(_pidPosDefault.kd * _pidAccuracy);
_out[i].ch[j].kdx =(unsigned short) _pidAccuracy; // D gain = 2/1 = 2
_out[i].ch[j].ki = (signed short)(_pidPosDefault.ki * _pidAccuracy);
_out[i].ch[j].kix = (unsigned short)_pidAccuracy; // I gain = 0/1 = 0
}
}
}
// motion done accuracy
_posAccuracy = config.check("motionDoneAccuracy",
Value(1000),
"Integer value specifying +/- range of encoder tics").asInt();
// encoder zeros
// limit exceedings
double *encoderZeros = new double[_intNumControllers];
_limitsExceeding = new bool[_intNumControllers];
for (int i = 0; i < _intNumControllers; i++){
encoderZeros[i] = 0.0;
_limitsExceeding[i] = false;
}
// Wrapper initialization
ImplementPositionControl<UrbtcControl, IPositionControl>::
initialize(_intNumControllers, axisMap, encoderFactors, encoderZeros);
ImplementVelocityControl<UrbtcControl, IVelocityControl>::
initialize(_intNumControllers, axisMap, encoderFactors, encoderZeros);
ImplementPidControl<UrbtcControl, IPidControl>::
initialize(_intNumControllers, axisMap, encoderFactors, encoderZeros);
ImplementEncoders<UrbtcControl, IEncoders>::
initialize(_intNumControllers, axisMap, encoderFactors, encoderZeros);
ImplementControlLimits<UrbtcControl, IControlLimits>::
initialize(_intNumControllers, axisMap, encoderFactors, encoderZeros);
// output devices found
printf("Number of urbtc devices found: %d\n", _intNumControllers);
// opening devices
for (int i = 0; i < _intNumControllers; i++){ // for all found devices
printf("Trying to open ");
puts (_deviceFilename[i]);
// open devicefile
if ( (_fd[i] = ::open(_deviceFilename[i], O_RDWR)) == -1 ){
printf("Error opening device number: %d\n", i);
return false;
}
// init ccmd
// position
_cmd[i].retval = 0;
_cmd[i].setoffset = CH0 | CH1 | CH2 | CH3;
_cmd[i].setcounter = CH0 | CH1 | CH2 | CH3;
_cmd[i].resetint = CH0 | CH1 | CH2 | CH3;
_cmd[i].selin = SET_SELECT | SET_CH2_HIN;
_cmd[i].dout = 0;
_cmd[i].selout = SET_SELECT | CH2;
_cmd[i].offset[0] = _cmd[i].offset[1] = _cmd[i].offset[2] = _cmd[i].offset[3] = _intOffset;
_cmd[i].counter[0] = _cmd[i].counter[1] = _cmd[i].counter[2] = _cmd[i].counter[3] = 0;
_cmd[i].posneg = SET_POSNEG | CH0 | CH1 | CH2 | CH3; /*POS PWM out*/
_cmd[i].breaks = SET_BREAKS | CH0 | CH1 | CH2 | CH3; /*No Brake*/
_cmd[i].magicno = 0x00;
_cmd[i].wrrom = 0; /* WR_MAGIC | WR_OFFSET | WR_SELOUT;*/
// do some ioctl stuff
if (ioctl(_fd[i], URBTC_COUNTER_SET) < 0) {
printf("Error on ioctl() for device number: %d\n",i);
return false;
}
// write the cmd struct out to the device
if (write(_fd[i], &_cmd[i], sizeof(_cmd[i])) < 0) {
printf("Error writing cmd struct to device number: %d\n",i);
return false;
}
// do some more ioctl() stuff
if (ioctl(_fd[i], URBTC_DESIRE_SET) < 0) {
printf("Error ioctl() URBTC_DESIRE_SET\n");
return false;
}
if (ioctl(_fd[i], URBTC_CONTINUOUS_READ) < 0) {
printf("Error ioctl() URBTC_CONTINUOUS_READ\n");
return false;
}
if (ioctl(_fd[i], URBTC_BUFREAD) < 0) {
printf("Error ioctl() URBTC_BUFREAD\n");
return false;
}
int test;
if ((test = read(_fd[i], &_in[i], sizeof(_in[i]))) != sizeof(_in[i])) {
printf("Warning: read size mismatch %d != %d\n", test, sizeof(_in[i]));
}
printf("Successfully opened.\n");
}
Thread::start();
return true;
}
/* Plugin: Multithreaded loop. */
void InputPlugin::operator()()
{
try
{
/* ZMQ: Create framework subscription socket on this thread. */
zmq::socket_t zmq_framework_subscriber (*this->zmq_context.get(), ZMQ_SUB);
/* ZMQ: Connect. */
zmq_framework_subscriber.connect("inproc://Framework");
/* ZMQ: Suscribe to all messages. */
zmq_framework_subscriber.setsockopt(ZMQ_SUBSCRIBE, "", 0);
std::this_thread::sleep_for(std::chrono::milliseconds( 10 ));
/* OIS: Initialization.*/
OIS::ParamList pl;
std::ostringstream wnd;
wnd << this->base_window.get()->native_window();
pl.insert(std::make_pair(std::string("WINDOW"), wnd.str()));
pl.insert(std::make_pair(std::string("x11_mouse_grab"), std::string("false")));
pl.insert(std::make_pair(std::string("x11_mouse_hide"), std::string("false")));
pl.insert(std::make_pair(std::string("x11_keyboard_grab"), std::string("false")));
auto ois_manager = OIS::InputManager::createInputSystem(pl);
/* OIS: Print debugging information. */
auto ois_info = std::string("-----OIS-----\n") +
"Version: " + ois_manager->getVersionName() + "\n" +
"Manager: " + ois_manager->inputSystemName() + "\n" +
"Total keyboards: " + std::to_string(ois_manager->getNumberOfDevices(OIS::OISKeyboard)) + "\n" +
"Total mice: " + std::to_string(ois_manager->getNumberOfDevices(OIS::OISMouse)) + "\n" +
"Total gamepads: " + "-" + "\n" +
"Total joysticks: " + std::to_string(ois_manager->getNumberOfDevices(OIS::OISJoyStick));
std::cout << ois_info << std::endl;
/* OIS: Keyboard initialization. */
auto ois_keyboard = static_cast<OIS::Keyboard*>(ois_manager->createInputObject( OIS::OISKeyboard, false));
/* ZMQ: Send ready message. */
{
zeug::string_hash message("Ready");
zmq::message_t zmq_message_send(message.Size());
memcpy(zmq_message_send.data(), message.Get(), message.Size());
this->zmq_input_publisher->send(zmq_message_send);
}
/* ZMQ: Listen for start message. */
for(;;)
{
zmq::message_t zmq_message;
if (zmq_framework_subscriber.recv(&zmq_message, ZMQ_NOBLOCK))
{
if (zeug::string_hash("Start") == zeug::string_hash(zmq_message.data()))
{
break;
}
}
}
/* Plugin: Loop. */
for(;;)
{
/* ZMQ: Listen for stop message. */
{
zmq::message_t zmq_message;
if (zmq_framework_subscriber.recv(&zmq_message, ZMQ_NOBLOCK))
{
if (zeug::string_hash("Stop") == zeug::string_hash(zmq_message.data()))
{
break;
}
}
}
if (this->base_window->poll())
{
/* OIS: Handle input */
ois_keyboard->capture();
if( ois_keyboard->isKeyDown( OIS::KC_ESCAPE ))
{
/* Topic */
{
zeug::string_hash message("Keyboard");
zmq::message_t zmq_message(message.Size());
memcpy(zmq_message.data(), message.Get(), message.Size());
zmq_input_publisher->send(zmq_message, ZMQ_SNDMORE);
}
/* Message */
{
zeug::string_hash message("Esc");
zmq::message_t zmq_message(message.Size());
memcpy(zmq_message.data(), message.Get(), message.Size());
zmq_input_publisher->send(zmq_message, ZMQ_SNDMORE);
}
/* End of message. */
{
zeug::string_hash message("Finish");
zmq::message_t zmq_message(message.Size());
memcpy(zmq_message.data(), message.Get(), message.Size());
zmq_input_publisher->send(zmq_message);
}
}
else if( ois_keyboard->isKeyDown( OIS::KC_RETURN ))
{
/* Topic */
{
zeug::string_hash message("Keyboard");
zmq::message_t zmq_message(message.Size());
memcpy(zmq_message.data(), message.Get(), message.Size());
zmq_input_publisher->send(zmq_message, ZMQ_SNDMORE);
}
/* Message */
{
zeug::string_hash message("Enter");
zmq::message_t zmq_message(message.Size());
memcpy(zmq_message.data(), message.Get(), message.Size());
zmq_input_publisher->send(zmq_message, ZMQ_SNDMORE);
}
/* End of message. */
{
zeug::string_hash message("Finish");
zmq::message_t zmq_message(message.Size());
memcpy(zmq_message.data(), message.Get(), message.Size());
zmq_input_publisher->send(zmq_message);
}
}
}
}
}
/* Plugin: Catch plugin specific exceptions and rethrow them as runtime error*/
catch(const OIS::Exception& ois_exception )
{
/* ZMQ: Send stop message. */
zeug::string_hash message("Stop");
zmq::message_t zmq_message_send(message.Size());
memcpy(zmq_message_send.data(), message.Get(), message.Size());
this->zmq_input_publisher->send(zmq_message_send);
throw std::runtime_error(std::string(" OIS - ") + ois_exception.eText);
return;
}
}
