diff --git a/shared/comps/curpid.c b/shared/comps/curpid.c index adf53021..fcfc8775 100644 --- a/shared/comps/curpid.c +++ b/shared/comps/curpid.c @@ -6,6 +6,9 @@ HAL_COMP(curpid); +// enable +HAL_PIN(en); + // current command HAL_PIN(id_cmd); HAL_PIN(iq_cmd); @@ -102,13 +105,20 @@ static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst ctx->iq_error_sum = LIMIT(ctx->iq_error_sum + kpq * kiq * iq_error, max_volt - uq); } else{ - ctx->id_error_sum = 0; - ctx->iq_error_sum = 0; + ctx->id_error_sum = 0.0; + ctx->iq_error_sum = 0.0; } ud += ctx->id_error_sum; uq += ctx->iq_error_sum; + if(PIN(en) <= 0.0){ + ud = 0.0; + uq = 0.0; + ctx->id_error_sum = 0.0; + ctx->iq_error_sum = 0.0; + } + PIN(ud) = ud; PIN(uq) = uq; diff --git a/stm32f303/src/main.c b/stm32f303/src/main.c index 41b8b614..7229a581 100644 --- a/stm32f303/src/main.c +++ b/stm32f303/src/main.c @@ -346,6 +346,7 @@ int main(void) hal_parse("curpid0.max_cur = ls0.max_cur"); hal_parse("curpid0.pwm_volt = ls0.pwm_volt"); hal_parse("curpid0.vel = ls0.vel"); + hal_parse("curpid0.en = ls0.en"); // hal parse config // hal_init_nrt();