mirror of https://github.com/rene-dev/stmbl.git
153 lines
3.5 KiB
C
153 lines
3.5 KiB
C
#include "commands.h"
|
|
#include "common.h"
|
|
#include "hal.h"
|
|
#include "math.h"
|
|
#include "defines.h"
|
|
#include "angle.h"
|
|
|
|
HAL_COMP(curpid);
|
|
|
|
// enable
|
|
HAL_PIN(en);
|
|
HAL_PIN(cmd_mode);
|
|
|
|
// current command
|
|
HAL_PIN(id_cmd);
|
|
HAL_PIN(iq_cmd);
|
|
|
|
// current feedback
|
|
HAL_PIN(id_fb);
|
|
HAL_PIN(iq_fb);
|
|
|
|
// voltage output
|
|
HAL_PIN(ud);
|
|
HAL_PIN(uq);
|
|
|
|
// maximum output current and voltage
|
|
HAL_PIN(max_cur);
|
|
HAL_PIN(pwm_volt);
|
|
|
|
// d, q resistance and inductance
|
|
HAL_PIN(rd);
|
|
HAL_PIN(rq);
|
|
HAL_PIN(ld);
|
|
HAL_PIN(lq);
|
|
|
|
// torque constant
|
|
HAL_PIN(psi);
|
|
|
|
HAL_PIN(ff); // r feed forward
|
|
HAL_PIN(kp);
|
|
HAL_PIN(ki);
|
|
HAL_PIN(kind); // bemf feed forward
|
|
|
|
HAL_PIN(vel); // velocity input
|
|
|
|
// current error outputs
|
|
HAL_PIN(id_error);
|
|
HAL_PIN(iq_error);
|
|
|
|
struct curpid_ctx_t {
|
|
float id_error_sum;
|
|
float iq_error_sum;
|
|
};
|
|
|
|
static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
|
|
// struct curpid_ctx_t * ctx = (struct curpid_ctx_t *)ctx_ptr;
|
|
struct curpid_pin_ctx_t *pins = (struct curpid_pin_ctx_t *)pin_ptr;
|
|
|
|
PIN(rd) = 0.5;
|
|
PIN(rq) = 0.5;
|
|
PIN(ld) = 0.01;
|
|
PIN(lq) = 0.01;
|
|
PIN(psi) = 0.05;
|
|
PIN(kp) = 0.1;
|
|
PIN(ki) = 0.005;
|
|
}
|
|
|
|
static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
|
|
struct curpid_ctx_t *ctx = (struct curpid_ctx_t *)ctx_ptr;
|
|
struct curpid_pin_ctx_t *pins = (struct curpid_pin_ctx_t *)pin_ptr;
|
|
|
|
float rd = MAX(PIN(rd), 0.1);
|
|
float rq = MAX(PIN(rq), 0.1);
|
|
float ld = MAX(PIN(ld), 0.001);
|
|
float lq = MAX(PIN(lq), 0.001);
|
|
|
|
float ff = PIN(ff);
|
|
float kind = PIN(kind);
|
|
float kpd = ld * PIN(kp) / period / 2.0;
|
|
float kid = rd * PIN(ki) / ld;
|
|
float kpq = lq * PIN(kp) / period / 2.0;
|
|
float kiq = rq * PIN(ki) / lq;
|
|
|
|
//TODO curpid: sqrt(di^2+qi^2) auf max_ac_cur clampen
|
|
float max_cur = MAX(PIN(max_cur), 0.0);
|
|
float idc = LIMIT(PIN(id_cmd), max_cur);
|
|
float iqc = LIMIT(PIN(iq_cmd), max_cur);
|
|
|
|
float max_volt = PIN(pwm_volt);
|
|
|
|
float id = PIN(id_fb);
|
|
float iq = PIN(iq_fb);
|
|
|
|
float vel = PIN(vel);
|
|
float psi_d = ld * id + PIN(psi);
|
|
float psi_q = lq * iq;
|
|
float indd = vel * psi_q;
|
|
float indq = vel * psi_d;
|
|
|
|
float id_error = idc - id;
|
|
float iq_error = iqc - iq;
|
|
|
|
float ud = LIMIT(ff * rd * idc - kind * indd + kpd * id_error, max_volt);
|
|
float uq = LIMIT(ff * rq * iqc + kind * indq + kpq * iq_error, max_volt);
|
|
|
|
if(kpd * kid > 0.0 && kpq * kiq > 0.0) {
|
|
ctx->id_error_sum = LIMIT(ctx->id_error_sum + kpd * kid * id_error, max_volt - ud);
|
|
ctx->iq_error_sum = LIMIT(ctx->iq_error_sum + kpq * kiq * iq_error, max_volt - uq);
|
|
} else {
|
|
ctx->id_error_sum = 0.0;
|
|
ctx->iq_error_sum = 0.0;
|
|
}
|
|
|
|
ud += ctx->id_error_sum;
|
|
uq += ctx->iq_error_sum;
|
|
|
|
if(PIN(cmd_mode) == VOLT_MODE) {
|
|
ud = LIMIT(PIN(id_cmd), max_volt);
|
|
uq = LIMIT(PIN(iq_cmd), max_volt);
|
|
ctx->id_error_sum = 0.0;
|
|
ctx->iq_error_sum = 0.0;
|
|
id_error = 0.0;
|
|
iq_error = 0.0;
|
|
}
|
|
|
|
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;
|
|
|
|
PIN(id_error) = id_error;
|
|
PIN(iq_error) = iq_error;
|
|
}
|
|
|
|
hal_comp_t curpid_comp_struct = {
|
|
.name = "curpid",
|
|
.nrt = 0,
|
|
.rt = rt_func,
|
|
.frt = 0,
|
|
.nrt_init = nrt_init,
|
|
.rt_start = 0,
|
|
.frt_start = 0,
|
|
.rt_stop = 0,
|
|
.frt_stop = 0,
|
|
.ctx_size = sizeof(struct curpid_ctx_t),
|
|
.pin_count = sizeof(struct curpid_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
|
|
};
|