From b72d7132c3b3e489eadafaf22b9c8bd82475f294 Mon Sep 17 00:00:00 2001 From: Pavel Pisa Date: Sun, 11 Jan 2015 21:38:24 +0100 Subject: [PATCH] RoCoN: add current D component stabilization to zero for stepper motor with feedback. Signed-off-by: Pavel Pisa --- sw/app/rocon/appl_pxmc.c | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/sw/app/rocon/appl_pxmc.c b/sw/app/rocon/appl_pxmc.c index 95cab6e..62be6af 100644 --- a/sw/app/rocon/appl_pxmc.c +++ b/sw/app/rocon/appl_pxmc.c @@ -916,6 +916,12 @@ pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs) int pxmc_pxmcc_pwm2ph_out(pxmc_state_t *mcs) { + pxmc_rocon_state_t *mcsrc = pxmc_state2rocon_state(mcs); + int32_t cur_d_raw; + int32_t cur_q_raw; + + pxmcc_axis_get_cur_dq_filt_raw(mcs, &cur_d_raw, &cur_q_raw); + if(!(mcs->pxms_flg&PXMS_PTI_m) || !(mcs->pxms_flg&PXMS_PHA_m) || (mcs->pxms_flg&PXMS_PRA_m)) { @@ -940,6 +946,23 @@ pxmc_pxmcc_pwm2ph_out(pxmc_state_t *mcs) ene = mcs->pxms_ene; pwm_d = 0; + + if (mcs->pxms_flg & PXMS_PHA_m && + (mcs->pxms_flg & (PXMS_ENR_m | PXMS_ENO_m))) { + int cur_d; + int cur_d_req, cur_d_err; + int max_pwm = (pxmc_rocon_pwm_magnitude * mcs->pxms_me) >> 15; + + cur_d_req = 0; + + pxmcc_axis_cur_dq_raw2filt(&cur_d, cur_d_raw); + + cur_d_err = cur_d_req - cur_d; + + pxmcc_cur_ctrl_pi(&pwm_d, &mcsrc->cur_d_err_sum, cur_d_err, + mcsrc->cur_d_p, mcsrc->cur_d_i, max_pwm); + } + pwm_q = (pxmc_rocon_pwm_magnitude * ene) >> 15; pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q); -- 2.39.2