static void control_system_init_distance_angle(struct control_system *am)
{
float distance_Ki = 0.003;
float angle_Ki = 0.003;
ausbee_pid_init(&(am->pid_distance), 0.4, distance_Ki, 0);
ausbee_pid_init(&(am->pid_angle), 0.2, angle_Ki, 0);
ausbee_pid_set_error_sum_range(&(am->pid_distance), -80/distance_Ki, 80/distance_Ki);
ausbee_pid_set_error_sum_range(&(am->pid_angle), -10/angle_Ki, 10/angle_Ki);
ausbee_pid_set_output_range(&(am->pid_distance), -100, 100);
ausbee_pid_set_output_range(&(am->pid_angle), -100, 100);
// Quadramp setup
ausbee_quadramp_init(&(am->quadramp_distance));
ausbee_quadramp_init(&(am->quadramp_angle));
// Setting quadramp eval period to the control system period
ausbee_quadramp_set_eval_period(&(am->quadramp_distance), CONTROL_SYSTEM_PERIOD_S);
ausbee_quadramp_set_eval_period(&(am->quadramp_angle), CONTROL_SYSTEM_PERIOD_S);
ausbee_quadramp_set_2nd_order_vars(&(am->quadramp_distance),
DISTANCE_MAX_ACC,
DISTANCE_MAX_ACC); // Translation acceleration (in mm/s^2)
ausbee_quadramp_set_2nd_order_vars(&(am->quadramp_angle),
DEG2RAD(ANGLE_MAX_ACC_DEG),
DEG2RAD(ANGLE_MAX_ACC_DEG)); // Rotation acceleration (in rad/s^2)
control_system_set_speed_high(am);
// Initialise each control system manager
ausbee_cs_init(&(am->csm_distance));
ausbee_cs_init(&(am->csm_angle));
// Set reference filter
ausbee_cs_set_reference_filter(&(am->csm_distance), ausbee_quadramp_eval, (void*)&(am->quadramp_distance));
ausbee_cs_set_reference_filter(&(am->csm_angle), ausbee_quadramp_eval, (void*)&(am->quadramp_angle));
// Set measure functions
ausbee_cs_set_measure_fetcher(&(am->csm_distance), position_get_distance_mm, NULL);
ausbee_cs_set_measure_fetcher(&(am->csm_angle), position_get_angle_rad, NULL);
// We use a pid controller because we like it here at Eirbot
ausbee_cs_set_controller(&(am->csm_distance), ausbee_pid_eval, (void*)&(am->pid_distance));
ausbee_cs_set_controller(&(am->csm_angle), ausbee_pid_eval, (void*)&(am->pid_angle));
// Set processing command
ausbee_cs_set_process_command(&(am->csm_distance), control_system_set_distance_mm_diff, am);
ausbee_cs_set_process_command(&(am->csm_angle), control_system_set_angle_rad_diff, am);
control_system_set_distance_mm_diff(am, 0);
control_system_set_angle_rad_diff(am, 0);
}
static void control_system_init_distance_angle()
{
ausbee_pid_init(&(am.pid_distance), 0.04, 0.0005, 0.0);
ausbee_pid_init(&(am.pid_angle), 0.02, 0.0004, 0.0);
ausbee_pid_set_output_range(&(am.pid_distance), -100, 100);
ausbee_pid_set_output_range(&(am.pid_angle), -100, 100);
// Quadramp setup
ausbee_quadramp_init(&(am.quadramp_distance));
ausbee_quadramp_init(&(am.quadramp_angle));
// Setting quadramp eval period to the control system period
ausbee_quadramp_set_eval_period(&(am.quadramp_distance), CONTROL_SYSTEM_PERIOD_S);
ausbee_quadramp_set_eval_period(&(am.quadramp_angle), CONTROL_SYSTEM_PERIOD_S);
ausbee_quadramp_set_2nd_order_vars(&(am.quadramp_distance),
DISTANCE_MAX_ACC,
DISTANCE_MAX_ACC); // Translation acceleration (in mm/s^2)
ausbee_quadramp_set_2nd_order_vars(&(am.quadramp_angle),
DEG2RAD(ANGLE_MAX_ACC_DEG),
DEG2RAD(ANGLE_MAX_ACC_DEG)); // Rotation acceleration (in rad/s^2)
control_system_set_speed_high();
// Initialise each control system manager
ausbee_cs_init(&(am.csm_distance));
ausbee_cs_init(&(am.csm_angle));
// Set reference filter
ausbee_cs_set_reference_filter(&(am.csm_distance), ausbee_quadramp_eval, (void*)&(am.quadramp_distance));
ausbee_cs_set_reference_filter(&(am.csm_angle), ausbee_quadramp_eval, (void*)&(am.quadramp_angle));
// Set measure functions
ausbee_cs_set_measure_fetcher(&(am.csm_distance), position_get_distance_mm);
ausbee_cs_set_measure_fetcher(&(am.csm_angle), position_get_angle_rad);
// We use a pid controller because we like it here at Eirbot
ausbee_cs_set_controller(&(am.csm_distance), ausbee_pid_eval, (void*)&(am.pid_distance));
ausbee_cs_set_controller(&(am.csm_angle), ausbee_pid_eval, (void*)&(am.pid_angle));
// Set processing command
ausbee_cs_set_process_command(&(am.csm_distance), control_system_set_distance_mm_diff);
ausbee_cs_set_process_command(&(am.csm_angle), control_system_set_angle_rad_diff);
control_system_set_distance_mm_diff(0);
control_system_set_angle_rad_diff(0);
}
