pid.c

#define PWM_ELEMENTS 100
const uint32_t PWM_Buffer[PWM_ELEMENTS] = { // Sine Table
    0,  3,  6,  9,  12, 15, 18, 21, 24, 27, 30, 33, 36, 39, 42, 45, 48,
    50, 53, 56, 58, 61, 63, 66, 68, 70, 72, 75, 77, 79, 80, 82, 84, 86,
    87, 89, 90, 91, 92, 94, 95, 96, 96, 97, 98, 98, 99, 99, 99, 99, 100,
    99, 99, 99, 99, 98, 98, 97, 96, 96, 95, 94, 92, 91, 90, 89, 87, 86,
    84, 82, 80, 79, 77, 75, 72, 70, 68, 66, 63, 61, 58, 56, 53, 50, 48,
    45, 42, 39, 36, 33, 30, 27, 24, 21, 18, 15, 12, 9,  6,  3};

#define ADC_LEN 2
uint32_t ADCValue[ADC_LEN];

/* constants for PID */
const float Kp = 0.2f;
const float Ki = 0.01f;
const float Kd = 0.001f;

// Loop for current control
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
  static int32_t index = 0;
  static uint8_t highSide = 0;
  static float dt = 0.01f;
  static int32_t previous_error = 0;
  static int8_t integral = 0;

  if (htim == &htim9) {
    uint32_t current = ADCValue[0];
    uint32_t setpoint = ADCValue[1];
    uint32_t pwm = PWM_Buffer[index];

    index = (index + 1) % PWM_ELEMENTS;

    int32_t error = setpoint - current;
    integral = MAX(-127, MIN(127, integral + error * dt));
    int32_t derivative = (error - previous_error) / dt;
    int32_t output = Kp * error + Ki * integral + Kd * derivative;
    previous_error = error;

    // printf("Setpoint %ld, Current %ld\n\r", setpoint, current);
    // printf("E %ld, I %ld, D %ld, Output %ld\n\r", error, integral,
    // derivative, output);
    // uint32_t val = (uint32_t)(pwm * output);

    uint32_t val = (uint32_t)(MAX(0, output)) * (pwm / 100.0f);

    if (val <= 5) {
      val = 0;
    }

    val = MIN(99, val);

    if (highSide == 1) {
      TIM9->CCR1 = val;
      if (index >= PWM_ELEMENTS - 1) {
        TIM9->CCR1 = 0;
        highSide = 0;
        return;
      }
    } else {
      TIM9->CCR2 = val;
      if (index >= PWM_ELEMENTS - 1) {
        TIM9->CCR2 = 0;
        highSide = 1;
        return;
      }
    }
  }
}