diff --git a/Makefile b/Makefile index 4ceeef14..00daa09c 100644 --- a/Makefile +++ b/Makefile @@ -98,11 +98,12 @@ COMPS += shared/comps/velbuf.c COMPS += shared/comps/avg.c COMPS += shared/comps/mux.c COMPS += shared/comps/veltopos.c -COMPS += shared/comps/wobl.c +# COMPS += shared/comps/wobl.c COMPS += shared/comps/debounce.c COMPS += shared/comps/pos_filter.c COMPS += shared/comps/rl.c COMPS += shared/comps/mad.c +COMPS += shared/comps/sensorless.c SOURCES += $(COMPS) diff --git a/conf/sensorless_perske.txt b/conf/sensorless_perske.txt new file mode 100644 index 00000000..f355d558 --- /dev/null +++ b/conf/sensorless_perske.txt @@ -0,0 +1,38 @@ +link pid +link acim +link jog_cmd +link misc +conf0.r = 4.3 +conf0.l = 0.008 +conf0.max_ac_cur = 2 +load sensorless +sensorless0.rt_prio = 5 +sensorless0.r = conf0.r +sensorless0.l = conf0.l +sensorless0.ud = hv0.ud_fb +sensorless0.uq = hv0.uq_fb +sensorless0.id = hv0.id_fb +sensorless0.iq = hv0.iq_fb +term0.wave0 = sensorless0.udd +term0.wave1 = sensorless0.udd +term0.wave2 = sensorless0.ed +term0.wave3 = sensorless0.eq +term0.wave4 = sensorless0.vel +term0.gain0 = 10 +term0.gain1 = 10 +term0.gain2 = 10 +term0.gain3 = 10 +term0.gain4 = 0.5 +term0.gain5 = 0.5 +term0.gain6 = 0.5 +#link rl +hv0.d_cmd = 0.55 +hv0.q_cmd = 0 +hv0.pos = sensorless0.pos +sensorless0.drop = 1.5 +sensorless0.ki = 0.5 +sensorless0.min_vel = 20 +sensorless0.vel_boost = 0.2 +conf0.r = 4 +conf0.max_ac_cur = 6 +sensorless0.kb = 1 diff --git a/shared/comps/sensorless.c b/shared/comps/sensorless.c new file mode 100644 index 00000000..90fe196b --- /dev/null +++ b/shared/comps/sensorless.c @@ -0,0 +1,138 @@ +/* +* This file is part of the stmbl project. +* +* Copyright (C) 2013-2016 Rene Hopf +* Copyright (C) 2013-2016 Nico Stute +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You shou l have received a copy of the GNU General Public License +* along with this program. If not, see . +*/ + +#include "commands.h" +#include "hal.h" +#include "math.h" +#include "defines.h" +#include "angle.h" + + +HAL_COMP(sensorless); + +//in +HAL_PIN(r); +HAL_PIN(l); +HAL_PIN(drop); + +HAL_PIN(ki); +HAL_PIN(kb); +HAL_PIN(kl); +HAL_PIN(min_vel); +HAL_PIN(vel_boost); + +HAL_PIN(id); +HAL_PIN(iq); +HAL_PIN(ud); +HAL_PIN(uq); +HAL_PIN(udd); +HAL_PIN(uqd); + +//out +HAL_PIN(vel); +HAL_PIN(pos); + +HAL_PIN(ed); +HAL_PIN(eq); + +HAL_PIN(old_id); +HAL_PIN(old_iq); + +HAL_PIN(delta_id); +HAL_PIN(delta_iq); + +static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) { + // struct sensorless_ctx_t *ctx = (struct sensorless_ctx_t *)ctx_ptr; + struct sensorless_pin_ctx_t *pins = (struct sensorless_pin_ctx_t *)pin_ptr; + PIN(ki) = 0.1; + PIN(kb) = 1; + PIN(kl) = 0.75; + PIN(min_vel) = 3.0 * 2.0 * M_PI; + PIN(vel_boost) = 0.2; + PIN(drop) = 1.5; +} + +static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { + // struct sensorless_ctx_t *ctx = (struct sensorless_ctx_t *)ctx_ptr; + struct sensorless_pin_ctx_t *pins = (struct sensorless_pin_ctx_t *)pin_ptr; + + float l = MAX(PIN(l), 0.00001); + float r = MAX(PIN(r), 0.01); + + float ud = PIN(ud); + float uq = PIN(uq); + float id = PIN(id); + float iq = PIN(iq); + + float kb = CLAMP(PIN(kb), 0, 1); + float ki = CLAMP(PIN(ki), 0, 1); + float kl = CLAMP(PIN(kl), 0, 1); + + float vel = PIN(vel); + float pos = PIN(pos); + + // drop compensation + ud -= SIGN2(id, 0.1) * PIN(drop); + uq -= SIGN2(iq, 0.1) * PIN(drop); + + PIN(udd) = ud; + PIN(uqd) = uq; + + // lowpass current change + PIN(delta_id) = PIN(delta_id) * kl + (id - PIN(old_id)) * (1 - kl); + PIN(delta_iq) = PIN(delta_iq) * kl + (iq - PIN(old_iq)) * (1 - kl); + + PIN(old_id) = id; + PIN(old_iq) = iq; + + // calc bemf + float ed = ud - r * id - PIN(delta_id) * l / period + vel * l * iq * kb; + float eq = uq - r * iq - PIN(delta_iq) * l / period - vel * l * id * kb; + + // velocity compensation, bemf_d => 0 + vel -= SIGN2(eq, 1.0) * ed * ki; + + // startup boost + if(ABS(vel) < PIN(min_vel)){ + vel += SIGN2(id * iq, 0.1) * PIN(vel_boost); + } + + pos += vel * period; + + PIN(ed) = ed; + PIN(eq) = eq; + + PIN(vel) = vel; + PIN(pos) = mod(pos); +} + +hal_comp_t sensorless_comp_struct = { + .name = "sensorless", + .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 = 0, + .pin_count = sizeof(struct sensorless_pin_ctx_t) / sizeof(struct hal_pin_inst_t), +}; diff --git a/shared/defines.h b/shared/defines.h index ad3ef33a..e8a50064 100644 --- a/shared/defines.h +++ b/shared/defines.h @@ -43,6 +43,8 @@ extern "C" { #define MIN3(a, b, c) MIN(a, MIN(b, c)) #define MAX3(a, b, c) MAX(a, MAX(b, c)) +#define SIGN2(a, b) (CLAMP((a) / (b), -1, 1)) + #define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0])) #define FIELD_SIZEOF(t, f) (sizeof(((t *)0)->f))