AsservPID - Kasimashi/Systemes-embarques GitHub Wiki

Asservissement PID

image

https://blog.technic-achat.com/quest-ce-que-le-pid/#:~:text=Un%20r%C3%A9gulateur%20PID%20(Proportionnel%20Int%C3%A9gral,ferm%C3%A9e%20d'un%20syst%C3%A8me%20automatique.

http://w3.cran.univ-lorraine.fr/perso/hugues.garnier/Enseignement/Auto_num/G_PID_numerique.pdf

image

Code exemple

image

#define MOTOR_VEL_REFERENCE 40
int16_t motor1_vel;
int32_t encoder_position;
uint16_t timer_counter;
#define PID_MAX 8000
#define INTEGRAL_GAIN_MAX 20000000

typedef struct
{
 float p_gain;
 float i_gain;
 float d_gain;
 int16_t last_error;
 int32_t error_integral;
 int16_t output;
}pid_instance_int16;
#define MOVING_AVERAGE_LENGHT 100
typedef struct {
  int16_t buffer[MOVING_AVERAGE_LENGHT];
  uint16_t counter;
  int16_t out;
  int32_t sum;
} mov_aver_instance_int16;

void reset_average_filter(mov_aver_instance_int16* instance) {
 instance->counter=0;
 instance->out=0;
 instance->sum=0;
 for (int i=0; i<MOVING_AVERAGE_LENGHT; i++) {
   instance->buffer[i] = 0;
 }
}

void apply_average_filter(mov_aver_instance_int16* instance, int16_t input, int16_t * out) {

  instance -> sum += input - instance->buffer[instance->counter];
  instance -> buffer[instance -> counter] = input;
  instance->counter++;
  if (instance-->counter == MOVING_AVERAGE_LENGHT)
  {
    instance->counter = 0;
  }
  instance->out = instance -> sum / MOVING_AVERAGE_LENGTH;
  // Normalization
  * out = instance ->out;
}
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {

  static uint8_t flag_button = 0;
  if (flag_button == 0) {
    flag_button = 1;
    set_pid_gain(&pid_instance_mot1, 230,100,20);
     printf("Gain are set");
  
  } else {
    flag_button = 0;
    set_pid_gain(&pid_instance_mot1, 0,0,0);
     printf("Gain are unset");
  }
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  timer_counter = __HAL_TIM_GET_COUNTER(&htim3);
  
 //Measure velocity
  update_encoder(&enc_instance_mot1, &htim3);

  // Applying filter
  apply_average_filter(&filter_instance1, enc_instance_mot1.velocity, &motor1_vel);

  if (pid_instance_mot1.d_gain != 0 || pid_instance_mot1.i_gain != 0 || pid_instance_mot1.p_gain != 0)
  {
   // apply PID
   apply_pid(&pid_instance_mot1, MOTOR_VEL_REFERENCE - motor1_vel, UPDATE_RATE);
  // PWM
  if (pid_instance_mot1.output > 0) 
  {
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pid_instance_mot1.output);
    HAL_GPIO_WritePin(MOTOR1_DIR_GPIO_Port, MOTOR1,DIR_Pin, GPIO_PIN_SET);
  } else {
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, -pid_instance_mot1.output);
    HAL_GPIO_WritePin(MOTOR1_DIR_GPIO_Port, MOTOR1,DIR_Pin, GPIO_PIN_RESET);
  }
 } else {
  // DO NOT APPLY PID
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 2000);
 }
}

void set_pid_gain(pid_instance_int16 *pid_instance, float p, float i, float d)
{
  pid_instance -> p_gain = p;
  pid_instance ->i_gain = i;
  pid_instance  -> d_gain = d;
}

void reset_pid(pid_instance_int16 *pid_instance)
{
  pid_instance -> p_gain = 0;
  pid_instance ->i_gain = 0;
  pid_instance  -> d_gain = 0;
 pid_instance  -> error_integral= 0;
}

void apply_pid(pid_instance_int16 *pid_instance, int16_t input_error, uin16_t sampling_rate)
{
 pid_instance -> error_integral += input_error;
 if (pid_instance->error_integral > INTEGRAL_GAIN_MAX)
 {
    pid_instance->error_integral = INTEGRAL_GAIN_MAX;
 }
 if (pid_instance->error_integral < -INTEGRAL_GAIN_MAX)
 {
    pid_instance->error_integral = -INTEGRAL_GAIN_MAX;
 }
 pid_instance -> output = pied_instance->p_gain * input_error +
 pid_instance->i_gain * (pid_instance->error_integral) / sampling_rate +
 pid_instance->d_gain * sampling_rate * (input_error - pid_instance->last_error);

 if (pid_instance -> output >= PID_MAX) {
   pid_instance -> output = PID_MAX;
 }
  if (pid_instance -> output <= -PID_MAX) {
   pid_instance -> output = -PID_MAX;
 }
  pid_instance->last_error = input_error;
}

Reference