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