Exemplo n.º 1
0
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);
}
Exemplo n.º 2
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);
}
Exemplo n.º 3
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);
}