示例#1
0
void Irp6otmM2J::updateHook() {
  if (RTT::NewData == port_desired_motor_position_.read(motor_position_)) {
    mp2i(&motor_position_(0), &joint_position_(0));
    port_joint_position_.write(joint_position_);
    desired_motor_pos_initiated_ = true;
  } else if ((!desired_motor_pos_initiated_)
      && (RTT::NewData == port_motor_position_.read(motor_position_))) {
    mp2i(&motor_position_(0), &joint_position_(0));
    port_joint_position_.write(joint_position_);
  }
}
示例#2
0
void SarkofagJ2M::updateHook() {
	if(port_joint_position_.read(joint_position_) == RTT::NewData) {
	  if(i2mp(&joint_position_(0), &motor_position_(0))) {
	    port_motor_position_.write(motor_position_);
	  }
	}
}
示例#3
0
void HwModel::updateHook() {
  if (RTT::NewData == port_desired_input_.read(desired_input_)) {
//    std::cout << "HwModel updateHook" << desired_input_(1) << std::endl;
// pytanie czy to nie przychodzi w inkrementach
    for (int servo = 0; servo < number_of_servos_; servo++) {
      // PWM input do implementacji
      /*
       if (!current_input_[servo]) {  // pwm input
       motor_position_(servo) = desired_input_(servo);
       } else {  // current input
       */

      // prad jest w miliamperach
      desired_torque_(servo) = desired_input_(servo) * torque_constant_[servo];

      for (int iteration = 0; iteration < iteration_per_step_; iteration++) {
        effective_torque_(servo) = desired_torque_(servo)
            - motor_velocity_(servo) * viscous_friction_[servo];
        motor_acceleration_(servo) = effective_torque_(servo) / inertia_[servo];
        motor_velocity_(servo) += motor_acceleration_(servo) / m_factor_;
        motor_position_(servo) += motor_velocity_(servo) / m_factor_;
      }
      //}
    }
    port_motor_position_.write(motor_position_);
  }
}
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::updateHookInit() {
  for (int i = 0; i < number_of_drives_; i++) {
    if (RTT::NewData != computedReg_in_list_[i]->read(pwm_or_current_[i])) {
      RTT::log(RTT::Error) << "NO PWM DATA" << RTT::endlog();
    }
  }

  for (int i = 0; i < number_of_drives_; i++) {
    if (current_mode_[i]) {
      hi_->set_current(i, (double) pwm_or_current_[i]);
    } else {
      hi_->set_pwm(i, (double) pwm_or_current_[i]);
    }
  }

  hi_->write_read_hardware(rwh_nsec_, timeouts_to_print_);
  if (hi_->hardware_panic) {
    port_is_hardware_panic_.write(true);
    if (error_msg_hardware_panic_ == 0) {
      std::cout << RED << std::endl << getName() << " [error] hardware panic"
                << RESET << std::endl << std::endl;
      error_msg_hardware_panic_++;
    }
  } else {
    port_is_hardware_panic_.write(false);
  }
  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]);
  }

  bool emergency_stop;
  if (port_emergency_stop_.read(emergency_stop) == RTT::NewData) {
    std::cout << RED << std::endl << getName() << " emergency_stop" << RESET
              << std::endl << std::endl;
    if (emergency_stop) {
      hi_->set_hardware_panic();
    }
  }

  for (int i = 0; i < number_of_drives_; i++) {
    motor_current_(i) = static_cast<double>(hi_->get_current(i));
    motor_increment_[i] = hi_->get_increment(i);

    port_motor_current_list_[i]->write(static_cast<double>(motor_current_[i]));

    double tmp_pos = motor_position_[i] * (enc_res_[i] / (2.0 * M_PI));

    port_motor_position_list_[i]->write(static_cast<double>(tmp_pos));
    /*  port_motor_increment_list_[i]->write(
     static_cast<double>(motor_increment_[i]));
     */
  }
}
示例#6
0
void HwModel::updateHook() {
  for (int servo = 0; servo < number_of_servos_; servo++) {
    port_desired_input_list_[servo]->read(desired_input_[servo]);

    // prad jest w przeliczany do amperow
    desired_torque_(servo) = desired_input_(servo) * torque_constant_[servo]
        / input_current_units_per_amper_[servo];

    for (int iteration = 0; iteration < iteration_per_step_; iteration++) {
      effective_torque_(servo) = desired_torque_(servo)
          - motor_velocity_(servo) * viscous_friction_[servo];
      motor_acceleration_(servo) = effective_torque_(servo) / inertia_[servo];
      motor_velocity_(servo) += motor_acceleration_(servo) / m_factor_;
      motor_position_(servo) += motor_velocity_(servo) / m_factor_;
    }
    inc_motor_position_[servo] = motor_position_[servo] * enc_res_[servo]
        / (2.0 * M_PI);

    port_motor_position_list_[servo]->write(inc_motor_position_[servo]);
  }
}
示例#7
0
bool HwModel::configureHook() {
  number_of_servos_ = torque_constant_.size();
  if ((number_of_servos_ != inertia_.size())
      || (number_of_servos_ != viscous_friction_.size())
      || (number_of_servos_ != current_input_.size())) {
    std::cout
        << std::endl
        << RED
        << "[error] hardware model "
        << getName()
        << "configuration failed: wrong properties vector length in launch file."
        << RESET << std::endl;
    return false;
  }

  motor_position_.resize(number_of_servos_);
  motor_velocity_.resize(number_of_servos_);
  motor_acceleration_.resize(number_of_servos_);
  desired_input_.resize(number_of_servos_);
  desired_torque_.resize(number_of_servos_);
  effective_torque_.resize(number_of_servos_);

  port_motor_position_.setDataSample(motor_position_);

  for (int i = 0; i < number_of_servos_; i++) {
    motor_position_(i) = 0.0;
    motor_velocity_(i) = 0.0;
    motor_acceleration_(i) = 0.0;
    desired_input_(i) = 0.0;
    desired_torque_(i) = 0.0;
    effective_torque_(i) = 0.0;
  }

  m_factor_ = step_per_second_ * iteration_per_step_;

  return true;
}
void SarkofagM2J::updateHook() {
  if (RTT::NewData == port_motor_position_.read(motor_position_)) {
    mp2i(&motor_position_(0), &joint_position_(0));
    port_joint_position_.write(joint_position_);
  }
}
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]));
    }
  }
}
示例#10
0
void Irp6otmM2J::updateHook() {
  port_motor_position_.read(motor_position_);
  mp2i(&motor_position_(0), &joint_position_(0));
  port_joint_position_.write(joint_position_);
}
示例#11
0
bool HwModel::configureHook() {
  number_of_servos_ = torque_constant_.size();
  if ((number_of_servos_ != torque_constant_.size())
      || (number_of_servos_ != input_current_units_per_amper_.size())
      || (number_of_servos_ != inertia_.size())
      || (number_of_servos_ != viscous_friction_.size())
      || (number_of_servos_ != enc_res_.size())) {
    std::cout
        << std::endl
        << RED
        << "[error] hardware model "
        << getName()
        << " configuration failed: wrong properties vector length in launch file."
        << RESET << std::endl;
    return false;
  }

  motor_position_.resize(number_of_servos_);
  motor_velocity_.resize(number_of_servos_);
  motor_acceleration_.resize(number_of_servos_);
  desired_input_.resize(number_of_servos_);
  desired_torque_.resize(number_of_servos_);
  effective_torque_.resize(number_of_servos_);
  inc_motor_position_.resize(number_of_servos_);

  // port_motor_position_.setDataSample(motor_position_);

  port_motor_position_list_.resize(number_of_servos_);
  port_desired_input_list_.resize(number_of_servos_);

  desired_position_out_list_.resize(number_of_servos_);
  port_motor_position_command_list_.resize(number_of_servos_);
  port_motor_current_list_.resize(number_of_servos_);
  port_regulator_reset_list_.resize(number_of_servos_);

  for (int i = 0; i < number_of_servos_; i++) {
    char port_motor_position_name[32];
    snprintf(port_motor_position_name, sizeof(port_motor_position_name),
             "MotorPosition_%s", port_labels_[i].c_str());
    port_motor_position_list_[i] = new typeof(*port_motor_position_list_[i]);
    this->ports()->addPort(port_motor_position_name,
                           *port_motor_position_list_[i]);

    char port_desired_input_name[32];
    snprintf(port_desired_input_name, sizeof(port_desired_input_name),
             "computedReg_in_%s", port_labels_[i].c_str());
    port_desired_input_list_[i] = new typeof(*port_desired_input_list_[i]);
    this->ports()->addPort(port_desired_input_name,
                           *port_desired_input_list_[i]);

    char DesiredPosition_out_port_name[32];
    snprintf(DesiredPosition_out_port_name,
             sizeof(DesiredPosition_out_port_name), "DesiredPosition_out_%s",
             port_labels_[i].c_str());
    desired_position_out_list_[i] = new typeof(*desired_position_out_list_[i]);
    this->ports()->addPort(DesiredPosition_out_port_name,
                           *desired_position_out_list_[i]);

    char MotorPositionCommand_port_name[32];
    snprintf(MotorPositionCommand_port_name,
             sizeof(MotorPositionCommand_port_name), "MotorPositionCommand_%s",
             port_labels_[i].c_str());
    port_motor_position_command_list_[i] =
        new typeof(*port_motor_position_command_list_[i]);
    this->ports()->addPort(MotorPositionCommand_port_name,
                           *port_motor_position_command_list_[i]);

    char MotorCurrent_port_name[32];
    snprintf(MotorCurrent_port_name, sizeof(MotorCurrent_port_name),
             "MotorCurrent_%s", port_labels_[i].c_str());
    port_motor_current_list_[i] = new typeof(*port_motor_current_list_[i]);
    this->ports()->addPort(MotorCurrent_port_name,
                           *port_motor_current_list_[i]);

    char port_regulator_reset_name[32];
    snprintf(port_regulator_reset_name, sizeof(port_regulator_reset_name),
             "RegulatorResetOutput_%s", port_labels_[i].c_str());
    port_regulator_reset_list_[i] = new typeof(*port_regulator_reset_list_[i]);
    this->ports()->addPort(port_regulator_reset_name,
                           *port_regulator_reset_list_[i]);

    motor_position_(i) = 0.0;
    motor_velocity_(i) = 0.0;
    motor_acceleration_(i) = 0.0;
    desired_input_(i) = 0.0;
    desired_torque_(i) = 0.0;
    effective_torque_(i) = 0.0;
  }
  m_factor_ = step_per_second_ * iteration_per_step_;

  return true;
}