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());
}
}
}
