From eca2bfb910657a0035f05ef06fae65d9d52d849d Mon Sep 17 00:00:00 2001 From: crinq Date: Sun, 16 Sep 2018 18:21:57 +0200 Subject: [PATCH] simple iit --- conf/template/pid.txt | 9 +++- shared/comps/iit.c | 100 +++++++++++++++-------------------------- shared/comps/iit_old.c | 91 +++++++++++++++++++++++++++++++++++++ 3 files changed, 136 insertions(+), 64 deletions(-) create mode 100644 shared/comps/iit_old.c diff --git a/conf/template/pid.txt b/conf/template/pid.txt index 22be84f4..fec18371 100644 --- a/conf/template/pid.txt +++ b/conf/template/pid.txt @@ -10,6 +10,7 @@ load fault load hv load sim load io +load iit link conf hv0.rt_prio = 0.9 hv0.frt_prio = 1 @@ -23,6 +24,7 @@ vel2.rt_prio = 6 io0.rt_prio = 7 pid0.rt_prio = 8 fault0.rt_prio = 10 +iit0.rt_prio = 13 sim0.rt_prio = 13 term0.rt_prio = 16 rev0.rev = conf0.cmd_rev @@ -116,4 +118,9 @@ pe0.torque = pid0.torque_cor_cmd pe0.vel = vel1.vel pe0.r = conf0.r pe0.j = conf0.j -pe0.cap = 0.00054 \ No newline at end of file +pe0.cap = 0.00054 +iit0.max_temp = conf0.max_mot_temp +iit0.high_temp = conf0.high_mot_temp +iit0.max_cur = conf0.max_ac_cur +fault0.mot_temp = iit0.temp +iit0.cur = hv0.abs_cur \ No newline at end of file diff --git a/shared/comps/iit.c b/shared/comps/iit.c index 4ac14eb8..a2cb727d 100644 --- a/shared/comps/iit.c +++ b/shared/comps/iit.c @@ -2,90 +2,64 @@ #include "hal.h" #include "math.h" #include "defines.h" -#include "angle.h" HAL_COMP(iit); -HAL_PIN(brake_r); -HAL_PIN(brake); -HAL_PIN(air_temp); -HAL_PIN(mot_air_res); -HAL_PIN(mot_cap); -HAL_PIN(mot_tau); -HAL_PIN(mot_temp); -HAL_PIN(mot_max_temp); -HAL_PIN(mot_res); -HAL_PIN(mot_psi); -HAL_PIN(mot_real_res); -HAL_PIN(mot_real_psi); -HAL_PIN(mot_cont_cur); -HAL_PIN(id); -HAL_PIN(iq); -HAL_PIN(flow); +// conf +HAL_PIN(amb_temp); +HAL_PIN(high_temp); +HAL_PIN(max_temp); +HAL_PIN(max_cur); +HAL_PIN(cur_boost); +HAL_PIN(max_time); + +// out +HAL_PIN(temp); +// in +HAL_PIN(cur); struct iit_ctx_t { - float mot_temp; + float e; }; static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) { struct iit_ctx_t *ctx = (struct iit_ctx_t *)ctx_ptr; struct iit_pin_ctx_t *pins = (struct iit_pin_ctx_t *)pin_ptr; - ctx->mot_temp = 25.0; - PIN(brake_r) = 50.0; - PIN(brake) = 2.0; - PIN(air_temp) = 20.0; - PIN(mot_air_res) = 0.0; - PIN(mot_cap) = 0.0; - PIN(mot_tau) = 300.0; - PIN(mot_temp) = 25.0; - PIN(mot_max_temp) = 130.0; - PIN(mot_res) = 1.0; - PIN(mot_psi) = 1.0; - PIN(mot_real_res) = 1.0; - PIN(mot_real_psi) = 1.0; - PIN(mot_cont_cur) = 2.15; - PIN(id) = 0.0; - PIN(iq) = 0.0; - PIN(flow) = 0.0; + PIN(amb_temp) = 30.0; + PIN(high_temp) = 80.0; + PIN(max_temp) = 100.0; + PIN(max_time) = 10.0; + PIN(cur_boost) = 3.0; + ctx->e = 0.0; } static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) { struct iit_ctx_t *ctx = (struct iit_ctx_t *)ctx_ptr; struct iit_pin_ctx_t *pins = (struct iit_pin_ctx_t *)pin_ptr; - float mot_real_res = PIN(mot_res) * (1 + 0.004 * (ctx->mot_temp - PIN(air_temp))); + float cur_n = PIN(max_cur) / MAX(PIN(cur_boost), 1.0); + float max_e = cur_n * cur_n * MAX(PIN(max_time), 0.1); - float mot_max_res = PIN(mot_res) * (1 + 0.004 * (PIN(mot_max_temp) - PIN(air_temp))); - float mot_air_res = (PIN(mot_max_temp) - PIN(air_temp)) / (PIN(mot_cont_cur) * PIN(mot_cont_cur) * mot_max_res); - ; - float mot_cap = PIN(mot_tau) / mot_air_res; + float temp = ctx->e / max_e * PIN(max_temp) + PIN(amb_temp); - float brake_flow = 24.0 * 24.0 / PIN(brake_r) * PIN(brake); - float flow = (PIN(id) * PIN(id) + PIN(iq) * PIN(iq)) * 3.0 / 2.0 * mot_real_res - (ctx->mot_temp - PIN(air_temp)) / mot_air_res; + float pin = PIN(cur) * PIN(cur); + float pout = (temp - PIN(amb_temp)) * cur_n * cur_n / (PIN(high_temp) - PIN(amb_temp)); - flow += brake_flow; + ctx->e += (pin - pout) * period; - ctx->mot_temp += flow / mot_cap * period; - - PIN(flow) = flow; - PIN(mot_air_res) = mot_air_res; - PIN(mot_cap) = mot_cap; - PIN(mot_temp) = ctx->mot_temp; - PIN(mot_real_res) = mot_real_res; - PIN(mot_real_psi) = PIN(mot_psi) * (1 - 0.0013 * (ctx->mot_temp - PIN(air_temp))); - ; + PIN(temp) = temp; } hal_comp_t iit_comp_struct = { - .name = "iit", - .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 iit_ctx_t), - .pin_count = sizeof(struct iit_pin_ctx_t) / sizeof(struct hal_pin_inst_t), + .name = "iit", + .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 iit_ctx_t), + .pin_count = sizeof(struct iit_pin_ctx_t) / sizeof(struct hal_pin_inst_t), }; diff --git a/shared/comps/iit_old.c b/shared/comps/iit_old.c new file mode 100644 index 00000000..4ac14eb8 --- /dev/null +++ b/shared/comps/iit_old.c @@ -0,0 +1,91 @@ +#include "commands.h" +#include "hal.h" +#include "math.h" +#include "defines.h" +#include "angle.h" + +HAL_COMP(iit); + +HAL_PIN(brake_r); +HAL_PIN(brake); +HAL_PIN(air_temp); +HAL_PIN(mot_air_res); +HAL_PIN(mot_cap); +HAL_PIN(mot_tau); +HAL_PIN(mot_temp); +HAL_PIN(mot_max_temp); +HAL_PIN(mot_res); +HAL_PIN(mot_psi); +HAL_PIN(mot_real_res); +HAL_PIN(mot_real_psi); +HAL_PIN(mot_cont_cur); +HAL_PIN(id); +HAL_PIN(iq); +HAL_PIN(flow); + +struct iit_ctx_t { + float mot_temp; +}; + +static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) { + struct iit_ctx_t *ctx = (struct iit_ctx_t *)ctx_ptr; + struct iit_pin_ctx_t *pins = (struct iit_pin_ctx_t *)pin_ptr; + ctx->mot_temp = 25.0; + PIN(brake_r) = 50.0; + PIN(brake) = 2.0; + PIN(air_temp) = 20.0; + PIN(mot_air_res) = 0.0; + PIN(mot_cap) = 0.0; + PIN(mot_tau) = 300.0; + PIN(mot_temp) = 25.0; + PIN(mot_max_temp) = 130.0; + PIN(mot_res) = 1.0; + PIN(mot_psi) = 1.0; + PIN(mot_real_res) = 1.0; + PIN(mot_real_psi) = 1.0; + PIN(mot_cont_cur) = 2.15; + PIN(id) = 0.0; + PIN(iq) = 0.0; + PIN(flow) = 0.0; +} + +static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) { + struct iit_ctx_t *ctx = (struct iit_ctx_t *)ctx_ptr; + struct iit_pin_ctx_t *pins = (struct iit_pin_ctx_t *)pin_ptr; + + float mot_real_res = PIN(mot_res) * (1 + 0.004 * (ctx->mot_temp - PIN(air_temp))); + + float mot_max_res = PIN(mot_res) * (1 + 0.004 * (PIN(mot_max_temp) - PIN(air_temp))); + float mot_air_res = (PIN(mot_max_temp) - PIN(air_temp)) / (PIN(mot_cont_cur) * PIN(mot_cont_cur) * mot_max_res); + ; + float mot_cap = PIN(mot_tau) / mot_air_res; + + float brake_flow = 24.0 * 24.0 / PIN(brake_r) * PIN(brake); + float flow = (PIN(id) * PIN(id) + PIN(iq) * PIN(iq)) * 3.0 / 2.0 * mot_real_res - (ctx->mot_temp - PIN(air_temp)) / mot_air_res; + + flow += brake_flow; + + ctx->mot_temp += flow / mot_cap * period; + + PIN(flow) = flow; + PIN(mot_air_res) = mot_air_res; + PIN(mot_cap) = mot_cap; + PIN(mot_temp) = ctx->mot_temp; + PIN(mot_real_res) = mot_real_res; + PIN(mot_real_psi) = PIN(mot_psi) * (1 - 0.0013 * (ctx->mot_temp - PIN(air_temp))); + ; +} + +hal_comp_t iit_comp_struct = { + .name = "iit", + .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 iit_ctx_t), + .pin_count = sizeof(struct iit_pin_ctx_t) / sizeof(struct hal_pin_inst_t), +};