sys id + sim

Makefile

@@ -120,6 +120,9 @@ SHARED_COMPS += shared/comps/fmove.c
 SHARED_COMPS += shared/comps/home.c
 SHARED_COMPS += shared/comps/en.c
 SHARED_COMPS += shared/comps/th.c
+#SHARED_COMPS += shared/comps/asm.c
+SHARED_COMPS += shared/comps/id.c
+SHARED_COMPS += shared/comps/motsim.c
 
 COMPS = $(SRC_COMPS) $(SHARED_COMPS)
 SOURCES += $(COMPS)

shared/comps/id.c

@@ -0,0 +1,427 @@
+#include "id_comp.h"
+#include "hal.h"
+#include "defines.h"
+#include "angle.h"
+
+HAL_COMP(id);
+
+HAL_PIN(d_cmd);
+HAL_PIN(q_cmd);
+HAL_PIN(com_pos);
+HAL_PIN(com_mode);
+HAL_PIN(cmd_mode);
+HAL_PIN(en);
+
+HAL_PIN(id_fb);
+HAL_PIN(iq_fb);
+HAL_PIN(ud_fb);
+HAL_PIN(uq_fb);
+HAL_PIN(pos_fb);
+HAL_PIN(vel_fb);
+HAL_PIN(acc_fb);
+
+HAL_PIN(state);
+HAL_PIN(timer);
+
+HAL_PIN(r);
+HAL_PIN(l);
+HAL_PIN(lm);
+HAL_PIN(t);
+
+HAL_PIN(pp);
+HAL_PIN(com_offset);
+HAL_PIN(fb_rev);
+
+HAL_PIN(max_torque);
+HAL_PIN(test_cur);
+HAL_PIN(test_vel);
+HAL_PIN(ki);
+
+HAL_PIN(freq);
+HAL_PIN(amp);
+HAL_PIN(min_pos);
+HAL_PIN(max_pos);
+HAL_PIN(max_vel);
+HAL_PIN(max_acc);
+HAL_PIN(pos);
+HAL_PIN(vel);
+HAL_PIN(acc);
+
+HAL_PIN(pi);
+HAL_PIN(fi);
+HAL_PIN(li);
+HAL_PIN(ji);
+HAL_PIN(di);
+
+HAL_PIN(fb_torque);
+HAL_PIN(ff_torque);
+HAL_PIN(torque);
+
+HAL_PIN(psi);
+HAL_PIN(friction);
+HAL_PIN(load);
+HAL_PIN(inertia);
+HAL_PIN(damping);
+
+HAL_PIN(pos_bw);
+HAL_PIN(vel_bw);
+HAL_PIN(cur_bw);
+HAL_PIN(torque_sum);
+HAL_PIN(id_error_sum);
+HAL_PIN(iq_error_sum);
+HAL_PIN(target);
+
+static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  //struct id_ctx_t * ctx = (struct id_ctx_t *)ctx_ptr;
+  struct id_pin_ctx_t *pins = (struct id_pin_ctx_t *)pin_ptr;
+  PIN(test_cur) = 2.0;
+  PIN(max_torque) = 3.0;
+  PIN(test_vel) = 50.0;
+  PIN(ki) = 0.5;
+  PIN(pi) = 1.0;
+  PIN(fi) = 0.001;
+  PIN(li) = 5.0;
+  PIN(ji) = 200.0;
+  PIN(di) = 10.0;
+  PIN(pos_bw) = 10.0;
+  PIN(vel_bw) = 50.0;
+  PIN(cur_bw) = 250.0;
+  PIN(max_vel) = 100.0;
+  PIN(max_acc) = 1000.0;
+  PIN(min_pos) = -15.0;
+  PIN(max_pos) = 15.0;
+}
+
+static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  //struct id_ctx_t * ctx = (struct id_ctx_t *)ctx_ptr;
+  struct id_pin_ctx_t *pins = (struct id_pin_ctx_t *)pin_ptr;
+
+  switch((int)(PIN(state) * 10.0 + 0.5)){
+    case 0:
+      PIN(r) = 0.1;
+      PIN(l) = 0.001;
+      PIN(lm) = 0.0;
+      PIN(t) = 0.0;
+      PIN(psi) = 0.055;
+      PIN(pp) = 3.0;
+      PIN(com_offset) = 0.0;
+      PIN(fb_rev) = 0.0;
+      PIN(inertia) = 0.00002;
+    break;
+
+    case 10: // r
+      printf("Measure r, l\n");
+      printf("the motor can move a bit\n");
+      printf("id0.state = 1.2 to start\n");
+      
+      PIN(state) = 1.1;
+      PIN(timer) = 0.0;
+      PIN(d_cmd) = 0.0;
+      PIN(q_cmd) = 0.0;
+      PIN(com_pos) = 0.0;
+      PIN(cmd_mode) = 0.0;
+    break;
+
+    case 14:
+      printf("conf0.r = %f\n", PIN(r));
+      printf("conf0.l = %f\n", PIN(l));
+      PIN(state) = 2.0;
+    break;
+
+    case 20: // pp, fb_rev, com_offset
+      printf("Measure com_offset, polepairs, fb_rev\n");
+      printf("the motor will move\n");
+      printf("id0.state = 2.2 to start\n");
+
+      PIN(state) = 2.1;
+      PIN(timer) = 0.0;
+      //PIN(d_cmd) = 0.0;
+      PIN(q_cmd) = 0.0;
+      PIN(com_pos) = 0.0;
+      PIN(cmd_mode) = 0.0;
+    break;
+
+    case 24: // pp, fb_rev, com_offset
+      printf("conf0.polepairs = %f\n", PIN(pp));
+      printf("conf0.com_offset = %f\n", PIN(com_offset));
+      if(PIN(fb_rev) > 0.0){
+        printf("conf0.fb_rev = 1\n");
+      }
+      PIN(state) = 3.0;
+    break;
+
+    case 30:
+      printf("Measure torque constant\n");
+      printf("the motor will move\n");
+      printf("id0.state = 3.2 to start\n");
+
+      PIN(state) = 3.1;
+      PIN(timer) = 0.0;
+      PIN(d_cmd) = 0.0;
+      PIN(q_cmd) = 0.0;
+      PIN(torque_sum) = 0.0;
+      PIN(id_error_sum) = 0.0;
+      PIN(iq_error_sum) = 0.0;
+      PIN(cmd_mode) = 0.0;
+    break;
+
+    case 33:
+      printf("conf0.psi = %f\n", PIN(psi));
+
+      PIN(state) = 4.0;
+    break;
+    
+    case 40:
+      printf("Measure friction, damping and inertia\n");
+      printf("the motor will move\n");
+      printf("id0.state = 4.2 to start\n");
+
+      PIN(state) = 4.1;
+      PIN(timer) = 0.0;
+      PIN(d_cmd) = 0.0;
+      PIN(q_cmd) = 0.0;
+      PIN(torque_sum) = 0.0;
+      PIN(id_error_sum) = 0.0;
+      PIN(iq_error_sum) = 0.0;
+      PIN(cmd_mode) = 0.0;
+      PIN(target) = PIN(min_pos);
+    break;
+
+    case 43:
+      printf("conf0.j = %f\n", PIN(inertia));
+      printf("conf0.friction = %f\n", PIN(friction));
+      printf("conf0.damping = %f\n", PIN(damping));
+      PIN(state) = 4.4;
+    break;
+  }
+ 
+}
+
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  //struct id_ctx_t * ctx = (struct id_ctx_t *)ctx_ptr;
+  struct id_pin_ctx_t *pins = (struct id_pin_ctx_t *)pin_ptr;
+
+  // if(ABS(PIN(id_fb)) + ABS(PIN(iq_fb)) > 5.0 * PIN(test_cur)){
+  //   PIN(state) = 0;
+  //   PIN(d_cmd) = 0.0;
+  //   PIN(q_cmd) = 0.0;
+  //   PIN(com_pos) = 0.0;
+  //   PIN(cmd_mode) = 0.0;
+  //   PIN(en) = 0.0;
+  // }
+
+  switch((int)(PIN(state) * 10.0 + 0.5)){
+    case 0:
+    break;
+
+    case 12: // r
+      PIN(en) = 1.0;
+      PIN(d_cmd) += PIN(ki) * period * PIN(r) * (PIN(test_cur) - PIN(id_fb));
+      PIN(r) = PIN(r) * 0.99 + PIN(ud_fb) / MAX(PIN(id_fb), 0.01) * 0.01;
+
+      PIN(timer) += period;
+      if(PIN(timer) >= 2.0){
+        PIN(timer) = 0.0;
+        PIN(state) = 1.3;
+        //PIN(d_cmd) = 0.0;
+        //PIN(en) = 0.0;
+      }
+    break;
+
+    case 13: // l
+      PIN(en) = 1.0;
+      float avg_test_volt = PIN(test_cur) * MAX(PIN(r), 0.01);
+
+      PIN(l) = PIN(l) * 0.995 + ABS(PIN(ud_fb) - avg_test_volt / 2.0) / MAX(ABS(PIN(id_fb) - PIN(test_cur)), 0.001) * period * 0.005;
+      if(PIN(d_cmd) < avg_test_volt){
+        PIN(d_cmd) = avg_test_volt * 1.5;
+      }
+      else{
+        PIN(d_cmd) = avg_test_volt * 0.5;
+      }
+
+      PIN(timer) += period;
+      if(PIN(timer) >= 1.0){
+        PIN(timer) = 0.0;
+        PIN(state) = 1.4;
+        PIN(d_cmd) = avg_test_volt;
+        PIN(en) = 0.0;
+      }
+    break;
+
+    case 22: // com_offset
+      PIN(en) = 1.0;
+      PIN(d_cmd) += PIN(ki) * period * PIN(r) * (PIN(test_cur) - PIN(id_fb));
+
+      PIN(timer) += period;
+      if(PIN(timer) >= 1.0){
+        PIN(com_offset) = -PIN(pos_fb);
+        PIN(state) = 2.3;
+        PIN(timer) = 0.0;
+      }
+    break;
+
+    case 23: // pp, fb_rev
+      PIN(d_cmd) += PIN(ki) * period * PIN(r) * (PIN(test_cur) - PIN(id_fb));
+      PIN(com_pos) += PIN(test_vel) * period;
+      PIN(com_pos) = mod(PIN(com_pos));
+
+      if(ABS(PIN(vel_fb)) > 0.1){
+        PIN(pp) = PIN(pp) * 0.99 + PIN(test_vel) / PIN(vel_fb) * 0.01;
+      }
+
+      PIN(timer) += period;
+      if(PIN(timer) >= 1.0){
+        PIN(timer) = 0.0;
+        PIN(en) = 0.0;
+        PIN(d_cmd) = 0.0;
+        
+        if(PIN(pp) < 0.0){
+          PIN(fb_rev) = 1.0;
+          PIN(pp) *= -1.0;
+        }
+        PIN(pp) = (int)(PIN(pp) + 0.5);
+
+        PIN(state) = 2.4;
+      }
+    break;
+
+    case 32: // psi
+      PIN(en) = 1.0;
+      PIN(com_pos) = mod((PIN(pos_fb) + PIN(com_offset)) * PIN(pp));
+
+      float vel_error = PIN(test_vel) - PIN(vel_fb);
+      PIN(torque_sum) += PIN(ki) * vel_error * period;
+      
+      float id_error = 0.0 - PIN(id_fb);
+      PIN(d_cmd) = PIN(r) * PIN(id_fb) + PIN(cur_bw) * PIN(l) * id_error;
+      PIN(id_error_sum) = PIN(id_error_sum) + PIN(cur_bw) * PIN(r) * id_error * period;
+      PIN(d_cmd) += PIN(id_error_sum);
+
+      float iq_error = PIN(vel_bw) * period * vel_error + PIN(torque_sum) - PIN(iq_fb);
+      PIN(q_cmd) = PIN(r) * PIN(iq_fb) + PIN(cur_bw) * PIN(l) * iq_error;
+      PIN(iq_error_sum) = PIN(iq_error_sum) + PIN(cur_bw) * PIN(r) * iq_error * period;
+      PIN(q_cmd) += PIN(iq_error_sum);
+
+
+      if(ABS(PIN(vel_fb)) > 0.1){
+        float psi  = (PIN(uq_fb) - PIN(iq_fb) * PIN(r)) / (PIN(vel_fb) * PIN(pp));
+        PIN(psi) = PIN(psi) * (1.0 - period / PIN(pi)) + psi * period / PIN(pi);
+      }
+
+      PIN(psi) = CLAMP(PIN(psi), 0.001, 1.0);
+
+      PIN(timer) += period;
+      if(PIN(timer) >= 5.0){
+        PIN(timer) = 0.0;
+        PIN(en) = 0.0;
+        PIN(d_cmd) = 0.0;
+        PIN(q_cmd) = 0.0;
+        PIN(id_error_sum) = 0.0;
+        PIN(iq_error_sum) = 0.0;
+
+        PIN(state) = 3.3;
+      }
+    break;
+
+    case 42: // j, d, f
+      if(PIN(timer) < 100.0){
+        PIN(freq) = PIN(max_vel) / (ABS(PIN(max_pos) - PIN(min_pos)) / 2.0 * 2.0 * M_PI);
+        PIN(freq) = MIN(PIN(freq), sqrtf(PIN(max_acc) / (ABS(PIN(max_pos) - PIN(min_pos)) / 2.0 * 4.0 * M_PI * M_PI)));
+
+        PIN(pos) = PIN(amp) * sinf(PIN(timer) * 2.0 * M_PI * PIN(freq));
+        PIN(vel) = PIN(amp) * 2.0 * M_PI * PIN(freq) * cosf(PIN(timer) * 2.0 * M_PI * PIN(freq));
+        PIN(acc) = -PIN(amp) * 4.0 * M_PI * M_PI * PIN(freq) * PIN(freq) * sinf(PIN(timer) * 2.0 * M_PI * PIN(freq));
+
+        PIN(amp) = LIMIT(PIN(amp) + (PIN(max_pos) - PIN(min_pos)) / 2.0 / 30.0 * period, (PIN(max_pos) - PIN(min_pos)) / 2.0);
+      }
+      else{
+        PIN(pos) += PIN(vel) * period + PIN(acc) * period * period / 2.0;
+        PIN(vel) += PIN(acc) * period;
+        float to_go = PIN(target) - PIN(pos);
+        float time_to_go = sqrtf(2.0 * ABS(to_go) / PIN(max_acc));
+        float acc = PIN(max_acc) * SIGN(to_go);
+        float vel = acc * time_to_go;
+        vel = LIMIT(vel, PIN(max_vel));
+        acc = (vel - PIN(vel)) / period;
+        PIN(acc) = LIMIT(acc, PIN(max_acc));
+
+        if(ABS(PIN(max_pos) - PIN(pos)) < 0.1){
+          PIN(target) = PIN(min_pos);
+        }
+        else if(ABS(PIN(min_pos) - PIN(pos)) < 0.1){
+          PIN(target) = PIN(max_pos);
+        }
+      }
+      
+      
+      float pos_p = PIN(pos_bw);
+      float vel_p = PIN(vel_bw);
+      float vel_i = PIN(vel_bw);
+
+      float pos_error = minus(mod(PIN(pos)), PIN(pos_fb));
+      float vel_cmd = pos_p * pos_error + PIN(vel);
+      vel_error = vel_cmd - PIN(vel_fb);
+      PIN(fb_torque) = LIMIT(vel_p * vel_error * PIN(inertia), PIN(max_torque));
+      PIN(torque_sum) = LIMIT(PIN(torque_sum) + vel_i * vel_error * period * PIN(inertia), PIN(max_torque) - PIN(fb_torque));
+      PIN(fb_torque) += PIN(torque_sum);
+      PIN(ff_torque) = PIN(load) + PIN(damping) * vel_cmd + PIN(friction) * SIGN2(vel_cmd, PIN(max_vel) * 0.001) + PIN(inertia) * PIN(acc);
+      PIN(torque) = LIMIT(PIN(fb_torque) + PIN(ff_torque), PIN(max_torque));
+
+
+      //PIN(load) += period / PIN(li) * PIN(fb_torque) * period;
+      PIN(damping) += period / PIN(di) * PIN(fb_torque) * PIN(vel) * period;
+      PIN(friction) += period / PIN(fi) * PIN(fb_torque) * SIGN(PIN(vel)) * period;
+      PIN(inertia) += period / PIN(ji) * PIN(fb_torque) * PIN(acc) * period;
+
+      //PIN(load) = CLAMP(PIN(load), -100.0, 100.0);
+      PIN(damping) = CLAMP(PIN(damping), 0.0, 100.0);
+      PIN(friction) = CLAMP(PIN(friction), 0.0, 100.0);
+      PIN(inertia) = CLAMP(PIN(inertia), 0.000005, 50.0);
+
+      PIN(en) = 1.0;
+      PIN(com_pos) = mod((PIN(pos_fb) + PIN(com_offset)) * PIN(pp));
+
+      id_error = 0.0 - PIN(id_fb);
+      PIN(d_cmd) = PIN(r) * PIN(id_fb) + PIN(cur_bw) * PIN(l) * id_error;
+      PIN(id_error_sum) = PIN(id_error_sum) + PIN(cur_bw) * PIN(r) * id_error * period;
+      PIN(d_cmd) += PIN(id_error_sum);
+
+      float q_cmd = PIN(torque) / 3.0 * 2.0 / PIN(psi) / PIN(pp);
+      iq_error = q_cmd - PIN(iq_fb);
+      PIN(q_cmd) = PIN(r) * PIN(iq_fb) + PIN(cur_bw) * PIN(l) * iq_error;
+      PIN(iq_error_sum) = PIN(iq_error_sum) + PIN(cur_bw) * PIN(r) * iq_error * period;
+      PIN(q_cmd) += PIN(iq_error_sum);
+      
+      PIN(timer) += period;
+      if(PIN(timer) > 150.0){
+        PIN(timer) = 0.0;
+        PIN(en) = 0.0;
+        PIN(d_cmd) = 0.0;
+        PIN(q_cmd) = 0.0;
+        PIN(id_error_sum) = 0.0;
+        PIN(iq_error_sum) = 0.0;
+        PIN(acc) = 0.0;
+        PIN(vel) = 0.0;
+        PIN(amp) = 0.0;
+
+        PIN(state) = 4.3;
+      }
+    break;
+  }
+}
+
+
+hal_comp_t id_comp_struct = {
+    .name      = "id",
+    .nrt       = nrt,
+    .rt        = rt_func,
+    .frt       = 0,
+    .nrt_init  = nrt_init,
+    .rt_start  = 0,
+    .frt_start = 0,
+    .rt_stop   = 0,
+    .frt_stop  = 0,
+    .ctx_size  = 0,//sizeof(struct id_ctx_t),
+    .pin_count = sizeof(struct id_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
+};

shared/comps/motsim.c

@@ -0,0 +1,239 @@
+#include "motsim_comp.h"
+#include "hal.h"
+#include "defines.h"
+#include "angle.h"
+
+HAL_COMP(motsim);
+
+
+// sim inputs
+HAL_PIN(d_cmd);
+HAL_PIN(q_cmd);
+
+HAL_PIN(cmd_mode);
+HAL_PIN(en);
+
+HAL_PIN(com_pos);
+HAL_PIN(com_vel);
+
+HAL_PIN(r);
+HAL_PIN(l);
+HAL_PIN(psi);
+
+HAL_PIN(load_torque); // load
+
+
+// sim outputs
+HAL_PIN(id_fb);
+HAL_PIN(ud_fb);
+HAL_PIN(iq_fb);
+HAL_PIN(uq_fb);
+
+HAL_PIN(pos_fb);
+
+// sim state
+HAL_PIN(pos);
+HAL_PIN(vel);
+HAL_PIN(acc);
+HAL_PIN(temp);
+HAL_PIN(id);
+HAL_PIN(iq);
+
+// sim config
+HAL_PIN(mot_r); // resistance
+HAL_PIN(mot_l); // inductance
+HAL_PIN(mot_psi); // torque constant
+
+HAL_PIN(mot_pp); // pole pairs
+HAL_PIN(mot_j); // inertia
+HAL_PIN(mot_f); // friction
+HAL_PIN(mot_d); // damping
+
+HAL_PIN(thermal_r); // thermal resistance
+HAL_PIN(thermal_mass); // thermal mass
+
+HAL_PIN(deadtime); // pwm deadtime
+HAL_PIN(drop); // pwm drop
+
+HAL_PIN(pwm_volt); // dc volt
+HAL_PIN(pwm_res); // pwm res
+
+HAL_PIN(cur_res); // current res
+HAL_PIN(cur_noise); // current noise
+
+HAL_PIN(fb_res); // fb res
+HAL_PIN(fb_noise); // fb noise
+HAL_PIN(fb_delay); // fb delay
+
+HAL_PIN(curpid_mult); // curpid iterations / cycle
+HAL_PIN(motsim_mult); // motsim iterations / curpid cycle
+
+
+HAL_PIN(id_error_sum);
+HAL_PIN(iq_error_sum);
+HAL_PIN(cur_bw);
+
+
+static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  //struct motsim_ctx_t * ctx = (struct motsim_ctx_t *)ctx_ptr;
+  struct motsim_pin_ctx_t *pins = (struct motsim_pin_ctx_t *)pin_ptr;
+  PIN(r) = 1.0;
+  PIN(l) = 0.001;
+  PIN(psi) = 0.055;
+  
+  PIN(mot_r) = 0.75;
+  PIN(mot_l) = 0.0015;
+  PIN(mot_psi) = 0.05;
+
+  PIN(mot_pp) = 3;
+  PIN(mot_j) = 0.000025;
+  PIN(mot_f) = 0.01;
+  PIN(mot_d) = 0.003;
+
+  PIN(thermal_r) = 0.1;
+  PIN(thermal_mass) = 0.1;
+
+  PIN(deadtime) = 0.0;
+  PIN(drop) = 0.0;
+
+  PIN(pwm_volt) = 320.0;
+  PIN(pwm_res) = 4800;
+
+  PIN(cur_res) = 0.01;
+  PIN(cur_noise) = 0.0;
+
+  PIN(fb_res) = 4096;
+  PIN(fb_noise) = 0.0;
+  PIN(fb_delay) = 0.0;
+
+  PIN(curpid_mult) = 3;
+  PIN(motsim_mult) = 3;
+
+  PIN(cur_bw) = 500.0;
+
+  PIN(temp) = 20.0;
+}
+
+
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  //struct motsim_ctx_t * ctx = (struct motsim_ctx_t *)ctx_ptr;
+  struct motsim_pin_ctx_t *pins = (struct motsim_pin_ctx_t *)pin_ptr;
+
+  float ud = 0.0;
+  float uq = 0.0;
+
+  float si = 0.0;
+  float co = 0.0; 
+  sincos_fast(mod(PIN(pos) * PIN(mot_pp)), &si, &co);
+  
+  // inverse park transformation
+  float ia = PIN(id) * co - PIN(iq) * si;
+  float ib = PIN(id) * si + PIN(iq) * co;
+
+  sincos_fast(PIN(com_pos), &si, &co);
+
+  // park transformation
+  PIN(id_fb) = ia * co + ib * si;
+  PIN(iq_fb) = -ia * si + ib * co;
+  
+  float kp;
+  float ki;
+
+  float id_error;
+  float iq_error;
+  
+  float psi_d;
+  float psi_q;
+
+  float ind_d;
+  float ind_q;
+
+  switch((int)PIN(cmd_mode)){
+    case 0: // voltage mode
+      ud = PIN(d_cmd);
+      uq = PIN(q_cmd);
+    break;
+
+    case 1: // current mode
+      kp = PIN(cur_bw) * PIN(l);
+      ki = PIN(cur_bw) * PIN(r);
+
+      id_error = PIN(d_cmd) - PIN(id_fb);
+      iq_error = PIN(q_cmd) - PIN(iq_fb);
+      
+      psi_d = PIN(l) * PIN(id_fb) + PIN(psi);
+      psi_q = PIN(l) * PIN(iq_fb);
+
+      ind_d = PIN(com_vel) * psi_q;
+      ind_q = PIN(com_vel) * psi_d;
+
+      ud = LIMIT(PIN(r) * PIN(id_fb) - ind_d + kp * id_error, PIN(pwm_volt));
+      uq = LIMIT(PIN(r) * PIN(iq_fb) + ind_q + kp * iq_error, PIN(pwm_volt));
+
+      PIN(id_error_sum) = LIMIT(PIN(id_error_sum) + ki * id_error * period, PIN(pwm_volt) - ud);
+      PIN(iq_error_sum) = LIMIT(PIN(iq_error_sum) + ki * iq_error * period, PIN(pwm_volt) - uq);
+      ud += PIN(id_error_sum);
+      uq += PIN(iq_error_sum);
+    break;
+  }
+
+  // pwm resolution, limit
+  ud = LIMIT((int)(ud / PIN(pwm_volt) * PIN(pwm_res)) / PIN(pwm_res) * PIN(pwm_volt), PIN(pwm_volt));
+  uq = LIMIT((int)(uq / PIN(pwm_volt) * PIN(pwm_res)) / PIN(pwm_res) * PIN(pwm_volt), PIN(pwm_volt));
+
+  PIN(ud_fb) = ud;
+  PIN(uq_fb) = uq;
+
+  // inverse park transformation
+  float ua = ud * co - uq * si;
+  float ub = ud * si + uq * co;
+
+  si = 0.0;
+  co = 0.0;
+  sincos_fast(mod(PIN(pos) * PIN(mot_pp)), &si, &co);
+
+  // park transformation
+  ud = ua * co + ub * si;
+  uq = -ua * si + ub * co;
+
+  // pwm distortion, deadtime, drop
+  //
+
+  if(PIN(en) <= 0.0){
+    ud = 0.0;
+    uq = 0.0;
+  }
+
+  psi_d = PIN(mot_l) * PIN(id) + PIN(mot_psi);
+  psi_q = PIN(mot_l) * PIN(iq);
+
+  ind_d = PIN(vel) * PIN(mot_pp) * psi_q;
+  ind_q = PIN(vel) * PIN(mot_pp) * psi_d;
+
+  PIN(id) += (ud - PIN(mot_r) * PIN(id) + ind_d) / PIN(mot_l) * period;
+  PIN(iq) += (uq - PIN(mot_r) * PIN(iq) - ind_q) / PIN(mot_l) * period;
+
+  float e_torque = 3.0 / 2.0 * PIN(mot_psi) * PIN(iq) * PIN(mot_pp);
+  float m_torque = e_torque + PIN(load_torque) - PIN(vel) * PIN(mot_d) - SIGN2(PIN(vel), 0.1) * PIN(mot_f);
+  PIN(acc) = m_torque / PIN(mot_j);
+  PIN(pos) += PIN(vel) * period + PIN(acc) * period * period / 2.0;
+  PIN(pos) = mod(PIN(pos));
+  PIN(vel) += PIN(acc) * period;  
+
+  PIN(pos_fb) = ((int)(PIN(pos) / 2.0 / M_PI * PIN(fb_res))) * 2.0 * M_PI / PIN(fb_res);
+}
+
+
+hal_comp_t motsim_comp_struct = {
+    .name      = "motsim",
+    .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,//sizeof(struct motsim_ctx_t),
+    .pin_count = sizeof(struct motsim_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
+};

src/comps/encm.c

@@ -160,7 +160,12 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
       case 65:
         cmd = 0x32; // request singel turn data
         expected_bytes = 9;
-      break;        
+      break;
+      
+      default:
+        cmd = 0x32;
+        expected_bytes = 9;
+      break;
     }
   }