sensorless

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)
 

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

shared/comps/sensorless.c

@@ -0,0 +1,138 @@
+/*
+* This file is part of the stmbl project.
+*
+* Copyright (C) 2013-2016 Rene Hopf <renehopf@mac.com>
+* Copyright (C) 2013-2016 Nico Stute <crinq@crinq.de>
+*
+* 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 <http://www.gnu.org/licenses/>.
+*/
+
+#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),
+};

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))