Exemplo n.º 1
0
void realrobot::writedevice()
{
    if(smach_state == "Power_On")
    {
        //ROS_INFO("POWER ON!");
        change_dxl_mode(rt_dynamixel_msgs::ModeSettingRequest::SETTING);
        set_torque(1);
        for(int i=0; i< total_dof; i++)
        {
            dxlJointSetPub.msg_.angle[i] = _desired_q(i);
        }
        if (dxlJointSetPub.trylock()) {
            dxlJointSetPub.unlockAndPublish();
        }
    }
    else if (smach_state == "Auto")
    {
        change_dxl_mode(rt_dynamixel_msgs::ModeSettingRequest::CONTROL_RUN);

        for(int i=0; i< total_dof; i++)
        {
            dxlJointSetPub.msg_.angle[i] = _desired_q(i);
        }
        if (dxlJointSetPub.trylock()) {
            dxlJointSetPub.unlockAndPublish();
        }
    }
    else if (smach_state == "Manual")
    {
        change_dxl_mode(rt_dynamixel_msgs::ModeSettingRequest::CONTROL_RUN);

        for(int i=0; i< total_dof; i++)
        {
            dxlJointSetPub.msg_.angle[i] = _desired_q(i);
        }
        if (dxlJointSetPub.trylock()) {
            dxlJointSetPub.unlockAndPublish();
        }
    }
    else if (smach_state == "None")
    {
        change_dxl_mode(rt_dynamixel_msgs::ModeSettingRequest::SETTING);
        set_torque(0);
    }
    else
    {
        for(int i=0; i< total_dof; i++)
        {
            dxlJointSetPub.msg_.angle[i] = _desired_q(i);
        }
        if (dxlJointSetPub.trylock()) {
            dxlJointSetPub.unlockAndPublish();
        }
    }
}
Exemplo n.º 2
0
void set_position(uint8_t position) {
	ref_target = position;
	while (get_position() != position) {
		set_torque(
			calculate_pd(G_Kp, G_Kd, position, mz_position, last_mz_position, time_delta_ms));
	}
}
Exemplo n.º 3
0
void set_speed(int16_t speed) {
	ref_target = speed;
	while (mz_velocity != speed) {
		set_torque(
			calculate_pd(G_Kp, G_Kd, speed, mz_velocity, last_mz_velocity, time_delta_ms));
	}
}
Exemplo n.º 4
0
Motor::Motor(GPIO_TypeDef * gpio_port,
             uint8_t direction_pin,
             uint8_t enable_pin,
             uint8_t fault_pin,
             stm32_tim_t * pwm_timer,
             uint8_t ccr_channel,
             float max_current,          // Amps
             float torque_constant,      // Newton meters per amp
             bool reverse_polarity)
    : current_{0.0f},
      torque_{0.0f},
      gpio_port_{gpio_port},
      pwm_timer_{pwm_timer},
      inv_torque_constant_{1.0f / torque_constant},
      max_current_{max_current},
      max_torque_{max_current * torque_constant},
      direction_pin_{direction_pin},
      enable_pin_{enable_pin},
      fault_pin_{fault_pin},
      ccr_channel_{ccr_channel},
      dir_negative_{reverse_polarity ? uint8_t(1) : uint8_t(0)},
      dir_positive_{reverse_polarity ? uint8_t(0) : uint8_t(1)}
{
    set_torque(0.0f);
    enable();
}
Exemplo n.º 5
0
Motor::~Motor()
{
    disable();
    set_torque(0.0f);
}