int epos_home_start(epos_node_t* node, const epos_home_t* home) {
unsigned int switch_vel = abs(epos_gear_from_angular_velocity(&node->gear,
home->switch_vel));
unsigned int zero_vel = abs(epos_gear_from_angular_velocity(&node->gear,
home->zero_vel));
unsigned int acc = abs(epos_gear_from_angular_acceleration(&node->gear,
home->acceleration));
int offset = epos_gear_from_angle(&node->gear, home->offset);
int pos = epos_gear_from_angle(&node->gear, home->position);
if (!epos_control_set_mode(&node->control, epos_control_homing) &&
!epos_home_set_method(&node->dev, home->method) &&
!epos_home_set_current_threshold(&node->dev, home->current*1e3) &&
!epos_home_set_switch_search_velocity(&node->dev, switch_vel) &&
!epos_home_set_zero_search_velocity(&node->dev, zero_vel) &&
!epos_home_set_acceleration(&node->dev, acc) &&
!epos_home_set_offset(&node->dev, offset) &&
!epos_home_set_position(&node->dev, pos) &&
!epos_profile_set_type(&node->dev, home->type) &&
!epos_device_set_control(&node->dev, EPOS_DEVICE_CONTROL_SHUTDOWN) &&
!epos_control_start(&node->control))
epos_device_set_control(&node->dev, EPOS_HOME_CONTROL_START);
return node->dev.error.code;
}
int epos_control_start(epos_control_p control) {
int result = epos_device_set_control(control->dev,
EPOS_DEVICE_CONTROL_ENABLE_OPERATION);
#ifdef __ELMO__
if (!result)
result = epos_device_set_control(control->dev,
EPOS_DEVICE_CONTROL_ENABLE_OPERATION);
#endif
if (!result)
timer_sleep(EPOS_CONTROL_START_SLEEP);
return result;
}
int epos_position_profile_start(epos_node_t* node, epos_position_profile_t*
profile) {
int pos = epos_gear_from_angle(&node->gear, profile->target_value);
unsigned int vel = abs(epos_gear_from_angular_velocity(&node->gear,
profile->velocity));
unsigned int acc = abs(epos_gear_from_angular_acceleration(&node->gear,
profile->acceleration));
unsigned int dec = abs(epos_gear_from_angular_acceleration(&node->gear,
profile->deceleration));
short control = (profile->relative) ?
EPOS_POSITION_PROFILE_CONTROL_SET_RELATIVE :
EPOS_POSITION_PROFILE_CONTROL_SET_ABSOLUTE;
if (!epos_control_set_mode(&node->control, epos_control_profile_pos) &&
!epos_position_profile_set_velocity(&node->dev, vel) &&
!epos_profile_set_acceleration(&node->dev, acc) &&
!epos_profile_set_deceleration(&node->dev, dec) &&
!epos_profile_set_type(&node->dev, profile->type) &&
!epos_control_start(&node->control) &&
!epos_position_profile_set_target(&node->dev, pos)) {
profile->start_value = epos_node_get_position(node);
error_return(&node->dev.error);
timer_start(&profile->start_time);
epos_device_set_control(&node->dev, control);
timer_correct(&profile->start_time);
}
return node->dev.error.code;
}
int epos_control_stop(epos_control_p control) {
return epos_device_set_control(control->dev,
EPOS_DEVICE_CONTROL_QUICK_STOP);
}
int epos_home_stop(epos_node_t* node) {
if (!epos_device_set_control(&node->dev, EPOS_HOME_CONTROL_HALT))
epos_control_stop(&node->control);
return node->dev.error.code;
}
