microchip-application-pec930-pec930_motor/app/src/motor_control.c

#include "motor_control.h"

#include "uvw_adc.h"

struct vf_control_formula_t g_vf_control_formulas[2] = {
  [VF_CTRL_FORMULA_STABLE] = {
                              .slope = VF_SLOPE_X10000,
                              .offset = (int32_t)(DDS_AMP_MAX * VF_OFFSET_STABLE_PCT),
                              },
  [VF_CTRL_FORMULA_MIN] = {
                              .slope = VF_SLOPE_X10000,
                              .offset = VF_OFFSET_MIN,
                              },
};

static enum amp_ftw_control_mode_e g_amp_ftw_control_mode = AMP_FTW_FIXED_FIXED;
static enum motor_mode_e g_motor_mode = MOTOR_MODE_RUN;

static int32_t div_round_closest(int32_t numerator, uint32_t denominator)
{
  if (denominator == 0U)
  {
    return 0;
  }

  if (numerator >= 0)
  {
    return (numerator + (int32_t)(denominator / 2U)) / (int32_t)denominator;
  }

  return (numerator - (int32_t)(denominator / 2U)) / (int32_t)denominator;
}

enum motor_mode_e motor_ctrl_get_mode(void)
{
  return g_motor_mode;
}

void motor_ctrl_set_amp_ftw_mode(enum amp_ftw_control_mode_e mode)
{
  g_amp_ftw_control_mode = mode;
}

void motor_ctrl_set_mode(enum motor_mode_e mode)
{
  g_motor_mode = mode;
}

void motor_ctrl_start(void)
{
  dds_pwm_start();
  adc_timer_start();
}

void motor_ctrl_stop(void)
{
  dds_pwm_stop();
  adc_timer_stop();
  dds_set_phase_acc_ftw(0);
  dds_set_phase_acc_amp(0);
  g_motor_mode = MOTOR_MODE_RUN;
  g_amp_ftw_control_mode = AMP_FTW_FIXED_FIXED;
}

static uint32_t motor_ctrl_vf_formula_cal(enum vf_control_use_type_e vf_control_use_type, int32_t dftw)
{
  struct vf_control_formula_t *formula = &g_vf_control_formulas[vf_control_use_type];
  int32_t damp = (dftw * formula->slope) / 10000 + formula->offset;

  if (damp < 0)
  {
    return 0;
  }

  return (uint32_t)damp > DDS_AMP_MAX ? DDS_AMP_MAX : (uint32_t)damp;
}

static void motor_ctrl_soft_start_mode(struct cmd_param_t *cmd_param)
{
  static int32_t dftw_step = 0;
  static int32_t dftw_goal = 0;
  static int32_t damp_first = 0;
  int32_t dftw = 0;
  uint32_t damp = 0;

  if (cmd_param->soft_start.dds_ftw_first == cmd_param->soft_start.dds_ftw_goal)
  {
    g_motor_mode = MOTOR_MODE_RUN;
    return;
  }

  if (g_amp_ftw_control_mode == AMP_FTW_FIXED_FIXED)
  {
    damp_first = (int32_t)cmd_param->soft_start.dds_amp_first;
    dftw_goal = cmd_param->soft_start.dds_ftw_goal;
    dftw_step = div_round_closest(cmd_param->soft_start.dds_ftw_goal - cmd_param->soft_start.dds_ftw_first,
                                  cmd_param->soft_start.soft_start_time_ms);
    if (dftw_step == 0 && cmd_param->soft_start.dds_ftw_goal != cmd_param->soft_start.dds_ftw_first)
    {
      dftw_step = (cmd_param->soft_start.dds_ftw_goal > cmd_param->soft_start.dds_ftw_first) ? 1 : -1;
    }
    g_vf_control_formulas[VF_CTRL_FORMULA_STABLE].offset = cmd_param->soft_start.curve_offset;
    dds_set_phase_acc_ftw((uint32_t)cmd_param->soft_start.dds_ftw_first);
    dds_set_phase_acc_amp(cmd_param->soft_start.dds_amp_first);
    g_amp_ftw_control_mode = AMP_FTW_FIXED_VARIABLE;
  }
  else if (g_amp_ftw_control_mode == AMP_FTW_FIXED_VARIABLE)
  {
    dds_get_ftw_amp_setting(&dftw, &damp);
    dftw += dftw_step;
    dds_set_phase_acc_ftw((uint32_t)dftw);
    if (motor_ctrl_vf_formula_cal(VF_CTRL_FORMULA_STABLE, dftw) >= (uint32_t)damp_first)
    {
      g_amp_ftw_control_mode = AMP_FTW_VARIABLE_VARIABLE;
    }
  }
  else if (g_amp_ftw_control_mode == AMP_FTW_VARIABLE_VARIABLE)
  {
    dds_get_ftw_amp_setting(&dftw, &damp);
    dftw += dftw_step;
    damp = motor_ctrl_vf_formula_cal(VF_CTRL_FORMULA_STABLE, dftw);
    dds_set_phase_acc_ftw((uint32_t)dftw);
    dds_set_phase_acc_amp(damp);

    if ((dftw_step >= 0 && dftw >= dftw_goal) || (dftw_step < 0 && dftw <= dftw_goal) || damp >= DDS_AMP_MAX)
    {
      g_motor_mode = MOTOR_MODE_RUN;
    }
  }
}

void motor_ctrl_init_cmd_param(struct cmd_param_t *cmd_param)
{
  cmd_param->dds_ftw = DDS_FTW_100HZ;
  cmd_param->dds_amp = DDS_AMP_100;
  cmd_param->notify_time_ms = 1000;
  cmd_param->notify_type = NOTIFY_TYPE_NODATA;
  cmd_param->vf_ctrl_fb_enabled = false;
  cmd_param->soft_start_enabled = false;
  cmd_param->soft_start.curve_offset = g_vf_control_formulas[VF_CTRL_FORMULA_STABLE].offset;
  cmd_param->soft_start.dds_amp_first = (DDS_AMP_MAX * 21U) / 100U;
  cmd_param->soft_start.dds_ftw_first = DDS_FTW_10HZ * 2;
  cmd_param->soft_start.dds_ftw_goal = DDS_FTW_10HZ * 5;
  cmd_param->soft_start.soft_start_time_ms = 5000;
}

void motor_ctrl_apply_default_settings(struct cmd_param_t *cmd_param)
{
  adc_duty_cycle_set(0.18f);
  dds_set_phase_acc_ftw((uint32_t)cmd_param->dds_ftw);
  dds_set_phase_acc_amp(cmd_param->dds_amp);
}

void motor_ctrl_update_task(struct cmd_param_t *cmd_param)
{
  static uint32_t update_damp_dftw_time = 0;
  uint32_t now = epwm_get_ms();

  if ((now - update_damp_dftw_time) >= 1U)
  {
    update_damp_dftw_time = now;

    if (g_motor_mode == MOTOR_MODE_SOFT_START)
    {
      motor_ctrl_soft_start_mode(cmd_param);
    }
  }

  if (dds_is_change_duty_flags())
  {
    dds_pwm_duty_cycle_update(cmd_param, g_motor_mode);
  }
}