double MotorTorqueController::execute (double _tau, double _tauMax) { // define controller state double dq, limitedTauRef; // define emergency state if (std::abs(_tau) > std::abs(_tauMax)) { if (m_emergencyController.state != ACTIVE) { // save transtion of current controller if (m_emergencyController.state != INACTIVE) { m_emergencyController.transition_dq = m_emergencyController.getMotorControllerDq(); } else if (m_normalController.state != INACTIVE) { m_emergencyController.transition_dq = m_normalController.getMotorControllerDq(); } m_emergencyController.dq = 0; m_emergencyController.controller->reset(); m_emergencyController.state = ACTIVE; } } else { if (m_emergencyController.state == ACTIVE && std::abs(_tau) <= std::max(std::abs(_tauMax) - TORQUE_MARGIN, 0.0)) { if (m_normalController.state != INACTIVE) { // take control over normal process m_normalController.transition_dq = m_emergencyController.getMotorControllerDq(); m_emergencyController.state = INACTIVE; } else { // activate stop process for emergency prepareStop(m_emergencyController); } } } // execute torque control and renew state limitedTauRef = std::min(std::max(-_tauMax, m_command_tauRef), _tauMax); updateController(_tau, limitedTauRef, m_normalController); dq = m_normalController.getMotorControllerDq(); if (m_emergencyController.state != INACTIVE) { // overwrite by tauMax control when emergency mode limitedTauRef = boost::math::copysign(_tauMax, _tau); updateController(_tau, limitedTauRef, m_emergencyController); dq = m_emergencyController.getMotorControllerDq(); } // for debug m_current_tau = _tau; m_actual_tauRef = limitedTauRef; return dq; }
void AllegroNodeSim::doIt(bool polling) { // Main spin loop, uses the publisher/subscribers. if (polling) { ROS_INFO("Polling = true."); while (ros::ok()) { updateController(); ros::spinOnce(); } } else { ROS_INFO("Polling = false."); // Timer callback (not recommended). ros::Timer timer = startTimerCallback(); ros::spin(); } }
void VPiano::customEvent ( QEvent *event ) { if (event->type() == NoteOnEventType ) { NoteOnEvent *ev = static_cast<NoteOnEvent*>(event); ui.pianokeybd->showNoteOn(ev->getNote()); if (m_midiThru) noteOn(ev->getNote()); event->accept(); } else if (event->type() == NoteOffEventType ) { NoteOffEvent *ev = static_cast<NoteOffEvent*>(event); ui.pianokeybd->showNoteOff(ev->getNote()); if (m_midiThru) noteOff(ev->getNote()); event->accept(); } else if (event->type() == ControllerEventType ) { ControllerEvent *ev = static_cast<ControllerEvent*>(event); updateController(ev->getController(), ev->getValue()); if (m_midiThru) sendController(ev->getController(), ev->getValue()); event->accept(); } }
// Interrupt-based control is not recommended by SimLab. I have not tested it. void AllegroNode::timerCallback(const ros::TimerEvent& event) { updateController(); }
void EvaluateTemplate::__evaluate() { _successfulEvaluation = false; while(_successfulEvaluation == false) { _lifeTime = Data::instance()->specification()->evaluation()->lifeTime(); _fitness = 0.0; try { if(_com == NULL) { _com = new YarsClientCom(); _com->throwException(true); stringstream sst; sst << _options << " " << _xml; _com->init(_workingDirectory, sst.str()); _nrOfSensors = _com->numberOfSensorsValues(); _nrOfActuators = _com->numberOfActuatorsValues(); cout << "Number of sensors: " << _nrOfSensors << endl; _sensorValues.resize(_nrOfSensors); _actuatorValues.resize(_nrOfActuators); } newIndividual(); for(int i = 0; i < _lifeTime; i++) { _com->update(); for(int j = 0; j < _nrOfSensors; j++) { _sensorValues[j] = _com->getSensorValue(j); } if(abort()) { _com->sendReset(); _successfulEvaluation = true; return; } updateController(); _rnn->setInputs(_networkInput); _rnn->update(); _rnn->getOutput(_actuatorValues); _message.str(""); _message << "Generation " << _population->generation() << "\n"; _message << "Fitness " << _fitness; updateFitnessFunction(); if(i % 10 == 0) { _com->sendMessage(_message.str()); } for(int j = 0; j < _nrOfActuators; j++) { _com->setActuatorValue(j, _actuatorValues[j]); } } _com->sendReset(); _successfulEvaluation = true; } catch(YarsClientComException &e) { _successfulEvaluation = false; stringstream sst; sst << _options << " " << _xml; _com->init(_workingDirectory, sst.str()); } } }