示例#1
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_);
  }
}
示例#2
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]);
  }
}
示例#3
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;
}
示例#4
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;
}