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();
}
}
}
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));
}
}
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));
}
}
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();
}
Motor::~Motor()
{
disable();
set_torque(0.0f);
}