tmcl_bldc_motor.cpp

Go to the documentation of this file.

 
#include "tmcl_bldc_motor.h"
 
 
/* Constructor */
BLDCMotor::BLDCMotor(ros::NodeHandle* p_nh, TmclInterpreter *p_tmcl_interpreter,
  uint16_t module_number, uint8_t motor_number) :
  Motor(p_nh, p_tmcl_interpreter, module_number, motor_number)
{
  b_statusflags_register_available_ = true;
 
  ROS_DEBUG_STREAM("[BLDCMotor::" <<  __func__ << "] called");
 
  /* Save StatusFlags RegisterNames and StatusFlags RegisterShift to vector */
  const std::string s_statusflags_reg_name = s_node_name_ + "/StatusFlags_Reg_name";
  if(!p_nh_->getParam(s_statusflags_reg_name, param_statusflags_reg_name_))
  {
    b_statusflags_register_available_ = false;
    ROS_WARN_STREAM("[" <<  __func__ << "] StatusFlags Register Names are not available.");
  }
  const std::string s_statusflags_reg_shift = s_node_name_ + "/StatusFlags_Reg_shift";
  if(b_statusflags_register_available_ && !p_nh_->getParam(s_statusflags_reg_shift, param_statusflags_reg_shift_))
  {
    b_statusflags_register_available_ = false;
    ROS_WARN_STREAM("[" <<  __func__ << "] StatusFlags Register Shift are not available.");
  }
 
  /* Check if Reg name and Reg shift vectors have the same length/size */
  if(b_statusflags_register_available_ && (param_statusflags_reg_name_.size() != param_statusflags_reg_shift_.size()))
  {
    b_statusflags_register_available_ = false;
    ROS_WARN_STREAM("[" <<  __func__ << "] Missing index for StatusFlagsRegisterName / StatusFlagsRegisterShift."
      "Check autogenerated YAML. StatusFlags wont be used!");
  }
}
 
/* Destructor */
BLDCMotor::~BLDCMotor()
{
  ROS_DEBUG_STREAM("[BLDCMotor::" <<  __func__ << "] called");
}
 
void BLDCMotor::init()
{
  int32_t val = 0;
 
  ROS_INFO_STREAM("[BLDCMotor::" << __func__ << "] called");
 
  if(p_tmcl_interpreter_->executeCmd(TMCL_CMD_GAP, "CommutationMode", motor_number_, &val))
  {
    comm_mode_ = (bldc_comm_mode_t) val;
  }
  else
  {
    comm_mode_ = BLDC_DISABLED_MOTOR;
    ROS_WARN_STREAM("[" << __func__ << "] \"CommutationMode\" is not available. Setting CommutationMode to Disable");
  }
 
  if(p_tmcl_interpreter_->executeCmd(TMCL_CMD_GAP, "PositionScalerM", motor_number_, &val))
  {
    position_scaler_ = val;
  }
  else
  {
    position_scaler_ = 0;
    ROS_WARN_STREAM("[" << __func__ << "] \"PositionScalerM\" is not available");
 
    if(comm_mode_ >= BLDC_CLOSEDLOOP_MOTOR && 
      p_tmcl_interpreter_->executeCmd(TMCL_CMD_GAP, "EncoderSteps", motor_number_, &val))
    {
      encoder_steps_ = val;
    }
    else
    {
      encoder_steps_ = 0;
      ROS_WARN_STREAM("[" << __func__ << "] \"EncoderSteps\" is not available.");
    }
  }
 
  /* Print units of each commands */
  if(param_wheel_diameter_ == 0)
  {
    ROS_INFO_STREAM("[" << __func__ << "] Velocity unit: rpm");
  }
  else
  {
    ROS_INFO_STREAM("[" << __func__ << "] Velocity unit: m/s");
  }
  if(position_scaler_ == 0 && encoder_steps_ == 0)
  {
    ROS_INFO_STREAM("[" << __func__ << "] Position unit: pulses");
  }
  else
  {
    ROS_INFO_STREAM("[" << __func__ << "] Position unit: angular degrees");
  }
  
  ROS_INFO_STREAM("[" << __func__ << "] Torque unit: mA");
 
  initPublisher();
  this->initSubscriber();
  ROS_INFO("[%s] Motor %d Initialized!\n", __func__, motor_number_);
}
 
/* Publisher Callback */
void BLDCMotor::rosPublishTmcInfo(const ros::TimerEvent& event)
{
  int32_t val = 0;
  int32_t status_flag = 0;
  uint8_t bin_index = 0;
  uint8_t array_index = 0;
  std::string binary = " ";
  std::string status = " ";
 
  tmc_info_msg_.header.stamp = ros::Time::now();
  tmc_info_msg_.header.seq = seq_ctr_;
  tmc_info_msg_.header.frame_id = frame_id_;
  tmc_info_msg_.interface_name = param_comm_interface_name_;
  tmc_info_msg_.motor_number = motor_number_;
 
  /* Initialize messages to 0 first */
  tmc_info_msg_.board_voltage = 0;
  tmc_info_msg_.status_flag = 0;
  tmc_info_msg_.velocity = 0;
  tmc_info_msg_.position = 0;
  tmc_info_msg_.torque = 0;
 
  if(p_tmcl_interpreter_->executeCmd(TMCL_CMD_GAP, "SupplyVoltage", motor_number_, &val))
  {
    tmc_info_msg_.board_voltage = val / 10; //converts mV to V
  }
  else
  {
    ROS_DEBUG_STREAM("[" << __func__ << "] Failed to get SupplyVoltage");
  }
 
  if(p_tmcl_interpreter_->executeCmd(TMCL_CMD_GAP, "StatusFlags", motor_number_, &val))
  {
    tmc_info_msg_.status_flag = val;
    status_flag = tmc_info_msg_.status_flag;
  }
  else
  {
    ROS_DEBUG_STREAM("[" << __func__ << "] Failed to get StatusFlags");
  }
 
  //Parsing StatusFlags value to display status in string
  if(b_statusflags_register_available_)
  {
    while(status_flag)
    {
      if(status_flag & 1)
      {
        binary+='1';
      }
      else
      {
        binary+='0';
      }
      status_flag>>=1;
    }
 
    for(bin_index = 0; bin_index < binary.length(); bin_index++)
    {
      if(binary[bin_index] == '1')
      {
        while(array_index < param_statusflags_reg_shift_.size())
        {
          if(bin_index == param_statusflags_reg_shift_[array_index])
          {
            status += param_statusflags_reg_name_[array_index - 1] + ", ";
            array_index = 0;
            break;
          }
          array_index++;
        }
      }
    }                    
    tmc_info_msg_.status = status; 
  }
  else
  {
    tmc_info_msg_.status = "StatusFlags Registers Not Available"; 
  }
 
  /* Velocity, Position, Torque */
  if(param_pub_actual_vel_)
  {
    if(p_tmcl_interpreter_->executeCmd(TMCL_CMD_GAP, "ActualVelocity", motor_number_, &val))
    {
      //converts rpm to linear velocity
      if(param_wheel_diameter_ == 0)
      {
        tmc_info_msg_.velocity = val * param_add_ratio_vel_;
      }
      else
      {
        tmc_info_msg_.velocity = val * ((PI * param_wheel_diameter_) / SECS_TO_MIN) * param_add_ratio_vel_;
      }
    }
    else
    {
      ROS_DEBUG_STREAM("[" << __func__ << "] Failed to get ActualVelocity");
    }
  }
 
  if(param_pub_actual_pos_)
  {
    if(p_tmcl_interpreter_->executeCmd(TMCL_CMD_GAP, "ActualPosition", motor_number_, &val))
    {
      //converts steps to degrees
      if(position_scaler_ > 0)
      {
        tmc_info_msg_.position = val * (ANGULAR_FULL_ROTATION / (float)position_scaler_) * param_add_ratio_pos_;
      }
      else if(encoder_steps_ > 0)
      {
        tmc_info_msg_.position = val * (ANGULAR_FULL_ROTATION / (float)encoder_steps_) * param_add_ratio_pos_;
      }
      else
      {
        tmc_info_msg_.position = val * param_add_ratio_pos_;
      }
    }
    else
    {
      ROS_DEBUG_STREAM("[" << __func__ << "] Failed to get ActualPosition");
    }
  }
 
  if(param_pub_actual_trq_)
  {
    if(p_tmcl_interpreter_->executeCmd(TMCL_CMD_GAP, "ActualTorque", motor_number_, &val))
    {
      tmc_info_msg_.torque = val * param_add_ratio_trq_;
    }
    else
    {
      ROS_DEBUG_STREAM("[" << __func__ << "] Failed to get ActualTorque");
    }
  }
 
  tmc_info_pub_.publish(tmc_info_msg_);
  seq_ctr_++;
}
 
/* Initialize Subscriber */
void BLDCMotor::initSubscriber()
{
  ROS_INFO_STREAM("[BLDCMotor::" << __func__ << "] called");
 
  if(comm_mode_ == BLDC_OPENLOOP_MOTOR)
  {
    ROS_INFO_STREAM("[" << __func__ << "] CommutationMode : OPENLOOP");
    tmc_cmd_vel_sub_ = p_nh_->subscribe(param_tmc_cmd_vel_topic_, 1000, &BLDCMotor::cmdVelCallback, this);
    tmc_cmd_abspos_sub_ = p_nh_->subscribe(param_tmc_cmd_abspos_topic_, 1000, &BLDCMotor::cmdAbsPosCallback, this);
    tmc_cmd_relpos_sub_ = p_nh_->subscribe(param_tmc_cmd_relpos_topic_, 1000, &BLDCMotor::cmdRelPosCallback, this);
  }
  else if(comm_mode_ >= BLDC_CLOSEDLOOP_MOTOR)
  {
    ROS_INFO_STREAM("[" << __func__ << "] CommutationMode : CLOSEDLOOP");
    Motor::initSubscriber();
  }
  /* Note: No subscriber if Commutation Mode is Disabled */
}
 
/* Subscriber Callback */
void BLDCMotor::cmdVelCallback(const geometry_msgs::Twist& msg)
{
  float val = msg.linear.x;
  int32_t board_val = 0;
 
  //If wheel diameter is set to 0 (or no wheels connected), the input value for linearX is equal to motors rpm 
  if(param_wheel_diameter_ == 0)
  {
    board_val = val / param_add_ratio_vel_;
  }
  else
  {
    //Formula to convert linear value to rpm (unit that the board accepts)
    board_val = val * (SECS_TO_MIN / (PI * param_wheel_diameter_)) * (1 / param_add_ratio_vel_);
  }
 
  ROS_DEBUG_STREAM("[" << __func__ << "] Subscriber callback entered, received: " << val
    << " board value: " << board_val);
 
  if(board_val >= 0)
  {
    if(p_tmcl_interpreter_->executeCmd(TMCL_CMD_ROR, 0, motor_number_, &board_val))
    {
      ROS_DEBUG_STREAM("\n[" << __func__ << "] Subscriber callback exited successfully");
    }
    else
    {
      ROS_ERROR_STREAM("[" << __func__ << "] Failed to set Velocity");
    }
  }
  else
  {
    board_val = abs(board_val);
    if(p_tmcl_interpreter_->executeCmd(TMCL_CMD_ROL, 0, motor_number_, &board_val))
    {
      ROS_DEBUG_STREAM("\n[" << __func__ << "] Subscriber callback exited successfully");
    }
    else
    {
      ROS_ERROR_STREAM("[" << __func__ << "] Failed to set Velocity");
    }
  } 
}
void BLDCMotor::cmdAbsPosCallback(const std_msgs::Int32 msg)
{
  float convert_const_deg = 0.00;
  int32_t unit_val = 0;
  int32_t val = msg.data;
 
  //convert input(degrees) to unit
  if(position_scaler_ > 0)
  {
    convert_const_deg = ((float)position_scaler_ / ANGULAR_FULL_ROTATION) * (1 / param_add_ratio_pos_);
  }
  else if(encoder_steps_ > 0)
  {
    convert_const_deg = ((float)encoder_steps_ / ANGULAR_FULL_ROTATION) * (1 / param_add_ratio_pos_);
  }
  else
  {
    //inverting position additional ratio 
    convert_const_deg = 1 / param_add_ratio_pos_;
  }
 
  unit_val = val * convert_const_deg;
 
  ROS_DEBUG_STREAM("[" << __func__ << "] Subscriber callback entered, received: " << val << " board value: "
    << unit_val);
 
  if(p_tmcl_interpreter_->executeCmd(TMCL_CMD_MVP, 0, motor_number_, &unit_val))
  {
    ROS_DEBUG_STREAM("\n[" << __func__ << "] Subscriber callback exited successfully");
  }
  else
  {
    ROS_ERROR_STREAM("[" << __func__ << "] Failed to set Absolute Position");
  }
}
void BLDCMotor::cmdRelPosCallback(const std_msgs::Int32 msg)
{
  float convert_const_deg = 0;
  int32_t unit_val = 0;
  int32_t val = msg.data;
 
  //convert input(degrees) to unit
  if(position_scaler_ > 0)
  {
    convert_const_deg = ((float)position_scaler_ / ANGULAR_FULL_ROTATION) * (1 / param_add_ratio_pos_);
  }
  else if(encoder_steps_ > 0)
  {
    convert_const_deg = ((float)encoder_steps_ / ANGULAR_FULL_ROTATION) * (1 / param_add_ratio_pos_);
  }
  else
  {
    //inverting position additional ratio 
    convert_const_deg = 1 / param_add_ratio_pos_;
  }
 
  unit_val = val *  convert_const_deg;
 
  ROS_DEBUG_STREAM("[" << __func__ << "] Subscriber callback entered, received: " << val << " board value: "
    << unit_val);
 
  if(p_tmcl_interpreter_->executeCmd(TMCL_CMD_MVP, 1, motor_number_, &unit_val))
  {
    ROS_DEBUG_STREAM("\n[" << __func__ << "] Subscriber callback exited successfully");
  }
  else
  {
    ROS_ERROR_STREAM("[" << __func__ << "] Failed to set Relative Position");
  }
}