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);
}
static void control_system_init_motors(struct control_system *am)
{
float Kp = 100. / (TICKS_PER_SECOND_AT_MAX_SPEED * CONTROL_SYSTEM_PERIOD_S);
float Ki = Kp * 0.1;
ausbee_pid_init(&(am->pid_right_motor), Kp, Ki, 0);
ausbee_pid_init(&(am->pid_left_motor), Kp, Ki, 0);
ausbee_diff_init(&(am->diff_right_motor));
ausbee_diff_init(&(am->diff_left_motor));
// Max error sum contribution to motors' speed command: 90
ausbee_pid_set_error_sum_range(&(am->pid_right_motor), -70/Ki, 70/Ki);
ausbee_pid_set_error_sum_range(&(am->pid_left_motor), -70/Ki, 70/Ki);
// Max motors' speed command: 100
ausbee_pid_set_output_range(&(am->pid_right_motor), -80, 80);
ausbee_pid_set_output_range(&(am->pid_left_motor), -80, 80);
// Initialise each control system manager
ausbee_cs_init(&(am->csm_right_motor));
ausbee_cs_init(&(am->csm_left_motor));
// Set measure functions
ausbee_cs_set_measure_fetcher(&(am->csm_right_motor), position_get_right_encoder, NULL);
ausbee_cs_set_measure_fetcher(&(am->csm_left_motor), position_get_left_encoder, NULL);
// Set measure filter (asservissement des moteurs en vitesse)
ausbee_cs_set_measure_filter(&(am->csm_right_motor), ausbee_diff_eval, (void*)&(am->diff_right_motor));
ausbee_cs_set_measure_filter(&(am->csm_left_motor), ausbee_diff_eval, (void*)&(am->diff_left_motor));
// We use a pid controller because we like it here at Eirbot
ausbee_cs_set_controller(&(am->csm_right_motor), ausbee_pid_eval, (void*)&(am->pid_right_motor));
ausbee_cs_set_controller(&(am->csm_left_motor), ausbee_pid_eval, (void*)&(am->pid_left_motor));
// Set processing command
ausbee_cs_set_process_command(&(am->csm_right_motor), motors_wrapper_right_motor_set_duty_cycle, NULL);
ausbee_cs_set_process_command(&(am->csm_left_motor), motors_wrapper_left_motor_set_duty_cycle, NULL);
}