bool HardwareInterfaceMW::startHook() {
try {
hi_->write_read_hardware(rwh_nsec_, 0);
servo_start_iter_ = 200;
for (int i = 0; i < number_of_drives_; i++) {
motor_position_(i) = static_cast<double>(hi_->get_position(i))
* ((2.0 * M_PI) / enc_res_[i]);
}
if (!hi_->robot_synchronized()) {
port_is_synchronised_.write(false);
RTT::log(RTT::Info) << "Robot not synchronized" << RTT::endlog();
synchro_start_iter_ = 500;
synchro_stop_iter_ = 1000;
synchro_state_ = MOVE_TO_SYNCHRO_AREA;
synchro_drive_ = 0;
if (auto_synchronize_) {
RTT::log(RTT::Info) << "Auto synchronize" << RTT::endlog();
std::cout << getName() << " Auto synchronize" << std::endl;
state_ = PRE_SYNCHRONIZING;
} else {
RTT::log(RTT::Info) << "Manual synchronize" << RTT::endlog();
std::cout << getName() << " Manual synchronize" << std::endl;
state_ = NOT_SYNCHRONIZED;
}
} else {
port_is_synchronised_.write(true);
RTT::log(RTT::Info) << "Robot synchronized" << RTT::endlog();
for (int i = 0; i < number_of_drives_; i++) {
motor_position_command_(i) = motor_position_(i);
motor_increment_(i) = static_cast<double>(hi_->get_increment(i));
motor_current_(i) = static_cast<double>(hi_->get_current(i));
}
state_ = PRE_SERVOING;
}
} catch (const std::exception& e) {
RTT::log(RTT::Error) << e.what() << RTT::endlog();
return false;
}
for (int i = 0; i < number_of_drives_; i++) {
port_regulator_reset_list_[i]->write(static_cast<double>(true));
desired_position_[i] = motor_position_(i);
}
return true;
}
void HardwareInterfaceMW::updateHookStateMachine() {
switch (state_) {
case NOT_SYNCHRONIZED: {
std_msgs::Bool do_synchro;
if (port_do_synchro_.read(do_synchro) == RTT::NewData) {
if (do_synchro.data) {
if (!test_mode_) {
state_ = PRE_SYNCHRONIZING;
}
std::cout << getName() << " NOT_SYNCHRONIZED" << std::endl;
}
}
}
break;
case PRE_SERVOING:
for (int i = 0; i < number_of_drives_; i++) {
motor_position_command_(i) = motor_position_(i);
port_motor_position_list_[i]->write(
static_cast<double>(motor_position_[i]));
}
if ((servo_start_iter_--) <= 0) {
state_ = SERVOING;
port_is_hardware_busy_.write(false);
std::cout << getName() << " Servoing started" << std::endl;
}
break;
case SERVOING: {
bool generator_active;
if (port_generator_active_.read(generator_active) == RTT::NewData) {
port_is_hardware_busy_.write(generator_active);
} else {
port_is_hardware_busy_.write(false);
}
for (int i = 0; i < number_of_drives_; i++) {
if (port_motor_position_command_list_[i]->read(
motor_position_command_[i]) == RTT::NewData) {
desired_position_[i] = motor_position_command_(i);
}
port_motor_position_list_[i]->write(
static_cast<double>(motor_position_[i]));
}
}
break;
case PRE_SYNCHRONIZING:
if ((synchro_start_iter_--) <= 0) {
state_ = SYNCHRONIZING;
std::cout << getName() << " Synchronization started" << std::endl;
}
break;
case SYNCHRONIZING:
switch (synchro_state_) {
case MOVE_TO_SYNCHRO_AREA:
if (synchro_needed_[synchro_drive_]) {
if (hi_->in_synchro_area(synchro_drive_)) {
RTT::log(RTT::Debug) << "[servo " << synchro_drive_
<< " ] MOVE_TO_SYNCHRO_AREA ended"
<< RTT::endlog();
synchro_state_ = STOP;
} else {
// ruszam powoli w stronę synchro area
RTT::log(RTT::Debug) << "[servo " << synchro_drive_
<< " ] MOVE_TO_SYNCHRO_AREA"
<< RTT::endlog();
desired_position_[synchro_drive_] +=
synchro_step_coarse_[synchro_drive_];
}
} else {
hi_->set_parameter_now(synchro_drive_, NF_COMMAND_SetDrivesMisc,
NF_DrivesMisc_SetSynchronized);
hi_->reset_position(synchro_drive_);
if (++synchro_drive_ == number_of_drives_) {
synchro_state_ = SYNCHRO_END;
} else {
synchro_state_ = MOVE_TO_SYNCHRO_AREA;
}
}
break;
case STOP:
hi_->start_synchro(synchro_drive_);
synchro_state_ = MOVE_FROM_SYNCHRO_AREA;
break;
case MOVE_FROM_SYNCHRO_AREA:
if (!hi_->in_synchro_area(synchro_drive_)) {
RTT::log(RTT::Debug) << "[servo " << synchro_drive_
<< " ] MOVE_FROM_SYNCHRO_AREA ended"
<< RTT::endlog();
synchro_state_ = WAIT_FOR_IMPULSE;
} else {
RTT::log(RTT::Debug) << "[servo " << synchro_drive_
<< " ] MOVE_FROM_SYNCHRO_AREA"
<< RTT::endlog();
desired_position_[synchro_drive_] +=
synchro_step_fine_[synchro_drive_];
}
break;
case WAIT_FOR_IMPULSE:
if (hi_->drive_synchronized(synchro_drive_)) {
RTT::log(RTT::Debug) << "[servo " << synchro_drive_
<< " ] WAIT_FOR_IMPULSE ended"
<< RTT::endlog();
hi_->finish_synchro(synchro_drive_);
hi_->reset_position(synchro_drive_);
if (++synchro_drive_ == number_of_drives_) {
synchro_state_ = SYNCHRO_END;
} else {
synchro_state_ = MOVE_TO_SYNCHRO_AREA;
}
} else {
RTT::log(RTT::Debug) << "[servo " << synchro_drive_
<< " ] WAIT_FOR_IMPULSE" << RTT::endlog();
desired_position_[synchro_drive_] +=
synchro_step_fine_[synchro_drive_];
}
break;
case SYNCHRO_END:
if ((synchro_stop_iter_--) == 1) {
for (int i = 0; i < number_of_drives_; i++) {
motor_position_command_(i) = motor_position_(i);
desired_position_[i] = motor_position_(i);
}
port_is_synchronised_.write(true);
RTT::log(RTT::Debug) << "[servo " << synchro_drive_
<< " ] SYNCHRONIZING ended" << RTT::endlog();
std::cout << getName() << " synchro finished" << std::endl;
} else if (synchro_stop_iter_ <= 0) {
state_ = PRE_SERVOING;
}
break;
}
break;
}
for (int i = 0; i < number_of_drives_; i++) {
if (state_ != SERVOING) {
desired_position_out_list_[i]->write(
static_cast<double>(desired_position_[i]));
}
}
}
