id fix

Makefile

@@ -121,8 +121,10 @@ 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/idp.c
+SHARED_COMPS += shared/comps/idpmsm.c
+SHARED_COMPS += shared/comps/iddc.c
 SHARED_COMPS += shared/comps/idm.c
+SHARED_COMPS += shared/comps/ids.c
 #SHARED_COMPS += shared/comps/motsim.c
 
 COMPS = $(SRC_COMPS) $(SHARED_COMPS)

conf/template/conf.txt

@@ -29,10 +29,10 @@ conf0.high_dc_volt = 350
 conf0.low_dc_volt = 24
 conf0.high_hv_temp = 70
 conf0.fan_hv_temp = 50
-conf0.pos_bw = 10
+conf0.pos_bw = 25
-conf0.vel_bw = 500
+conf0.vel_bw = 250
 conf0.vel_d = 5
 conf0.vel_g = 0.1
 conf0.torque_g = 1
-conf0.cur_bw = 1500
+conf0.cur_bw = 1000
 conf0.max_sat = 0.2

conf/template/dc.txt

@@ -1,18 +1,7 @@
-load dc_limits
 load dc_ttc
-dc_limits0.rt_prio = 7
 dc_ttc0.rt_prio = 9
-dc_limits0.r = conf0.r
-dc_limits0.ld = conf0.l
-dc_limits0.lq = conf0.l
-dc_limits0.psi = conf0.psi
 dc_ttc0.psi = conf0.psi
-dc_limits0.ac_volt = hv0.pwm_volt
+dc_ttc0.torque = pid0.torque_cmd
-dc_limits0.iq = hv0.iq_fb
-pid0.max_torque = dc_limits0.max_torque
-pid0.min_torque = dc_limits0.min_torque
-pid0.max_vel = dc_limits0.abs_max_vel
-dc_ttc0.torque = pid0.torque_cor_cmd
 hv0.d_cmd = 0
 hv0.pos = 0
 hv0.phase_mode = 3

conf/template/iddc.txt

@@ -0,0 +1,28 @@
+
+load iddc
+iddc0.rt_prio = 4
+
+iddc0.en = fault0.en_out
+fault0.pos_error = 0
+pid0.enable = 0
+hv0.en = iddc0.en_out
+hv0.cur_bw = iddc0.cur_bw
+hv0.cmd_mode = iddc0.cmd_mode
+hv0.d_cmd = iddc0.d_cmd
+hv0.q_cmd = iddc0.q_cmd
+hv0.pos = iddc0.com_pos
+hv0.rev = iddc0.out_rev
+hv0.r = idq0.r
+hv0.l = idq0.l
+hv0.phase_mode = 3
+iddc0.ud_fb = hv0.ud_fb
+iddc0.uq_fb = hv0.uq_fb
+iddc0.id_fb = hv0.id_fb
+iddc0.iq_fb = hv0.iq_fb
+iddc0.pwm_volt = hv0.pwm_volt
+
+iddc0.pos_fb = fb_switch0.pos_fb
+iddc0.vel_fb = vel1.vel
+
+vel0.pos_in = fb_switch0.pos_fb
+iddc0.vel_fb = vel1.vel

conf/template/idp.txt

@@ -1,27 +0,0 @@
-
-load idp
-idp0.rt_prio = 4
-
-idp0.en = fault0.en_out
-fault0.pos_error = 0
-pid0.enable = 0
-hv0.en = idp0.en_out
-hv0.cur_bw = idp0.cur_bw
-hv0.cmd_mode = idp0.cmd_mode
-hv0.d_cmd = idp0.d_cmd
-hv0.q_cmd = idp0.q_cmd
-hv0.pos = idp0.com_pos
-hv0.rev = idp0.out_rev
-hv0.r = idq0.r
-hv0.l = idq0.l
-idp0.ud_fb = hv0.ud_fb
-idp0.uq_fb = hv0.uq_fb
-idp0.id_fb = hv0.id_fb
-idp0.iq_fb = hv0.iq_fb
-idp0.pwm_volt = hv0.pwm_volt
-
-idp0.pos_fb = fb_switch0.pos_fb
-idp0.vel_fb = vel1.vel
-
-vel0.pos_in = fb_switch0.pos_fb
-idp0.vel_fb = vel1.vel

conf/template/idpmsm.txt

@@ -0,0 +1,28 @@
+
+load idpmsm
+idpmsm0.rt_prio = 4
+
+idpmsm0.en = fault0.en_out
+fault0.pos_error = 0
+fault0.sat_error = 0
+pid0.en = 0
+hv0.en = idpmsm0.en_out
+hv0.cur_bw = idpmsm0.cur_bw
+hv0.cmd_mode = idpmsm0.cmd_mode
+hv0.d_cmd = idpmsm0.d_cmd
+hv0.q_cmd = idpmsm0.q_cmd
+hv0.pos = idpmsm0.com_pos
+hv0.rev = idpmsm0.out_rev
+hv0.r = idq0.r
+hv0.l = idq0.l
+idpmsm0.ud_fb = hv0.ud_fb
+idpmsm0.uq_fb = hv0.uq_fb
+idpmsm0.id_fb = hv0.id_fb
+idpmsm0.iq_fb = hv0.iq_fb
+idpmsm0.pwm_volt = hv0.pwm_volt
+
+idpmsm0.pos_fb = fb_switch0.mot_abs_fb_no_offset
+idpmsm0.vel_fb = vel1.vel
+
+vel0.pos_in = fb_switch0.pos_fb
+idpmsm0.vel_fb = vel1.vel

conf/template/ids.txt

@@ -1,9 +1,37 @@
 
-link idm
+load ids
+ids0.rt_prio = 4
 
-idm0.sys = 1
+ids0.en = fault0.en_out
-pid0.j_mot = conf0.j
+pid0.pos_ext_cmd = ids0.pos_cmd
-pid0.j_sys = idm0.inertia
+pid0.vel_ext_cmd = ids0.vel_cmd
-pid0.d = idm0.damping
+pid0.acc_ext_cmd = ids0.acc_cmd
-pid0.f = idm0.friction
+
-pid0.l = idm0.load
+pid0.pos_bw = ids0.pos_bw
+pid0.vel_bw = ids0.vel_bw
+pid0.vel_d = ids0.vel_d
+
+ids0.pos_error = pid0.pos_error
+ids0.vel_error = pid0.vel_error
+
+ids0.cur_bw = conf0.cur_bw
+
+conf0.max_pos_error = 0
+conf0.max_sat = 10
+conf0.vel_g = 1
+
+term0.wave0 = ids0.pos_cmd
+term0.wave2 = ids0.vel_cmd
+term0.wave4 = ids0.min_cost
+term0.gain4 = 20
+#term0.wave1 = ids0.cost
+#term0.gain1 = 20
+
+term0.wave5 = ids0.pos_bw
+term0.gain5 = 1.0;
+term0.wave6 = ids0.vel_bw
+term0.gain6 = 0.1
+term0.wave7 = ids0.vel_d
+term0.gain7 = 10.0
+
+term0.send_step = 21

docs/src/cmd.adoc

@@ -0,0 +1,2 @@
+:lang: en
+

docs/src/feedback.adoc

@@ -0,0 +1,36 @@
+:lang: en
+
+= Feedback
+== incremental Encoder
+=== pinout
+=== config
+[source]
+link enc_fb0
+conf0.mot_fb_res = 4096
+
+== sin/cos Encoder
+=== pinout
+=== config
+[source]
+link enc_fb0
+conf0.mot_fb_res = 4096
+fb_switch0.mot_pos = enc_fb0.ipos
+
+
+== Resolver
+=== pinout
+=== config
+[source]
+link res_fb0
+
+== Tamagawa smartabs
+=== pinout
+=== config
+[source]
+link smartabs_fb0
+
+== Mitsubishi
+=== pinout
+=== config
+[source]
+link encm_fb0

docs/src/motor.adoc

@@ -0,0 +1,18 @@
+:lang: en
+
+= Motor
+== DC
+=== pinout
+=== config
+[source]
+link dc
+
+== PMSM
+=== pinout
+=== config
+[source]
+link pmsm
+
+== ASYNC
+=== pinout
+=== config

docs/src/pinout.adoc

@@ -0,0 +1,2 @@
+:lang: en
+

docs/src/servoterm.adoc

@@ -0,0 +1,3 @@
+:lang: en
+
+= servoterm

docs/src/supply.adoc

@@ -0,0 +1,8 @@
+:lang: en
+
+= Supply
+
+== LV supply
+24V 0.5A
+
+== HV supply

docs/src/tuning.adoc

@@ -0,0 +1,155 @@
+:lang: en
+
+= Parameter tuning
+
+== Requirements
+- link:supply.adoc[supply]
+- link:servoterm.adoc[servoterm]
+- link:feedback.adoc[feedback]
+- link:motor.adoc[motor]
+
+== electrical parameters
+disconnect the motor from the load
+
+=== dc motor
+==== manual tuning
+[source]
+conf0.r // motor phase resistance
+conf0.l // motor phase inductance
+conf0.psi // torque constant [Nm/A] or [V/rad/s]
+
+==== auto tuning
+this takes about 20 seconds
+the motor will move
+follow the instructions in Servoterm
+
+config:
+[source]
+link pid
+link dc
+link <your_feedback_type>
+link misc
+link iddc
+
+start:
+[source]
+fault0.en = 1
+
+example output:
+[source]
+OK fault0.en = 1.000000
+INFO: Enabled
+Measure r, l
+block the rotor
+id0.state = 1.2 to start
+OK id0.state = 1.200000
+conf0.r = 1.898350
+conf0.l = 0.013483
+Measure torque constant
+unblock the rotor
+the motor will move
+id0.state = 2.2 to start
+OK id0.state = 2.200000
+conf0.psi = 0.063680
+
+=== pmsm motor
+==== manual tuning
+[source]
+conf0.r // motor phase resistance
+conf0.l // motor phase inductance
+conf0.polecount // number of pole pairs
+conf0.mot_fb_offset
+conf0.mot_fb_rev
+conf0.psi // torque constant [Nm/A] or [V/rad/s]
+
+==== auto tuning
+this takes about 20 seconds
+the motor will move
+
+config:
+[source]
+link pid
+link pmsm
+link <your_feedback_type>
+link misc
+link idpmsm
+
+start:
+[source]
+fault0.en = 1
+
+example output:
+[source]
+OK fault0.en = 1.000000
+INFO: Enabled
+conf0.r = 1.898350
+conf0.l = 0.013483
+conf0.polecount = 4.000000
+conf0.mot_fb_offset = -0.258811
+conf0.psi = 0.063680
+
+== mechanical parameters
+connect the motor to the load
+
+=== manual tuning
+[source]
+conf0.j
+conf0.j_load
+
+=== auto tuning
+this takes about 3 minutes
+the motor will move
+
+config:
+[source]
+link pid
+link <your_motor_type>
+link <your_feedback_type>
+link misc
+link idm
+<your_electrical_parameters>
+
+start:
+[source]
+fault0.en = 1
+
+example output:
+[source]
+OK fault0.en = 1.000000
+INFO: Enabled
+conf0.j = 0.000085
+conf0.d = 0.000022
+conf0.f = 0.057124
+
+== control loop tuning
+=== manual tuning
+[source]
+conf0.pos_bw
+conf0.vel_bw
+conf0.vel_d
+
+=== auto tuning
+this takes about 2 minutes
+the motor will move
+
+config:
+[source]
+link pid
+link <your_motor_type>
+link <your_feedback_type>
+link misc
+link ids
+<your_electrical_parameters>
+<your_mechanical_parameters>
+
+start:
+[source]
+fault0.en = 1
+
+example output:
+[source]
+OK fault0.en = 1.000000
+INFO: Enabled
+conf0.pos_bw = 24.165899
+conf0.vel_bw = 450.000000
+conf0.vel_d = 6.058537

shared/comps/dc_ttc.c

@@ -20,7 +20,7 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   // struct dc_ttc_ctx_t * ctx = (struct dc_ttc_ctx_t *)ctx_ptr;
   struct dc_ttc_pin_ctx_t *pins = (struct dc_ttc_pin_ctx_t *)pin_ptr;
 
-  float psi_m  = MAX(PIN(psi), 0.01);
+  float psi_m  = MAX(PIN(psi), 0.001);
   float torque = PIN(torque);
 
   PIN(cur) = torque / psi_m;

shared/comps/fault.c

@@ -96,6 +96,19 @@ struct fault_ctx_t {
   float mot_temp_error;
 };
 
+void enable(char *ptr) {
+  hal_parse("fault0.en = 1");
+}
+
+COMMAND("enable", enable, "enable");
+
+void disable(char *ptr) {
+  hal_parse("fault0.en = 0");
+}
+
+COMMAND("disable", disable, "disable");
+
+
 static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   struct fault_ctx_t *ctx      = (struct fault_ctx_t *)ctx_ptr;
   struct fault_pin_ctx_t *pins = (struct fault_pin_ctx_t *)pin_ptr;

shared/comps/iddc.c

@@ -0,0 +1,245 @@
+#include "iddc_comp.h"
+#include "hal.h"
+#include "defines.h"
+#include "angle.h"
+
+HAL_COMP(iddc);
+
+HAL_PIN(d_cmd);
+HAL_PIN(q_cmd);
+HAL_PIN(com_pos);
+HAL_PIN(cmd_mode);
+HAL_PIN(en);
+HAL_PIN(en_out);
+
+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(state);
+HAL_PIN(timer);
+
+HAL_PIN(r);
+HAL_PIN(l);
+
+HAL_PIN(pp);
+HAL_PIN(com_offset);
+HAL_PIN(out_rev);
+
+HAL_PIN(test_cur);
+HAL_PIN(test_vel);
+HAL_PIN(ki);
+HAL_PIN(vel_bw);
+
+HAL_PIN(pi);
+
+HAL_PIN(pwm_volt);
+
+HAL_PIN(psi);
+
+HAL_PIN(cur_bw);
+HAL_PIN(cur_sum);
+
+HAL_PIN(tmp0);
+HAL_PIN(tmp1);
+HAL_PIN(tmp2);
+HAL_PIN(tmp3);
+HAL_PIN(avg_test_volt);
+
+static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  //struct iddc_ctx_t * ctx = (struct iddc_ctx_t *)ctx_ptr;
+  struct iddc_pin_ctx_t *pins = (struct iddc_pin_ctx_t *)pin_ptr;
+  PIN(test_cur) = 4.0;
+  PIN(test_vel) = 50.0;
+  PIN(ki) = 1.0;
+  PIN(pi) = 1.0;
+  PIN(vel_bw) = 20.0;
+  PIN(cur_bw) = 1.0;
+}
+
+static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  //struct iddc_ctx_t * ctx = (struct iddc_ctx_t *)ctx_ptr;
+  struct iddc_pin_ctx_t *pins = (struct iddc_pin_ctx_t *)pin_ptr;
+
+  switch((int)(PIN(state) * 10.0 + 0.5)){
+    case 0:
+      PIN(r) = 0.1;
+      PIN(l) = 0.001;
+      PIN(psi) = 0.055;
+      PIN(pp) = 3.0;
+      PIN(com_offset) = 0.0;
+      PIN(out_rev) = 0.0;
+      PIN(cur_bw) = 1.0;
+    break;
+
+    case 10: // r
+      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;
+
+      printf("Measure r, l\n");
+      printf("block the rotor\n");
+      printf("id0.state = 1.2 to start\n");
+    break;
+
+    case 14:
+      printf("conf0.r = %f # append to config\n", PIN(r));
+      printf("conf0.l = %f # append to config\n", PIN(l));
+      PIN(state) = 2.0;
+    break;
+
+    case 20:
+      PIN(state) = 3.1;
+      PIN(timer) = 0.0;
+      PIN(d_cmd) = 0.0;
+      PIN(q_cmd) = 0.0;
+      PIN(cur_sum) = 0.0;
+      PIN(cmd_mode) = 0.0;
+      PIN(cur_bw) = 250.0;
+
+      printf("Measure torque constant\n");
+      printf("unblock the rotor\n");
+      printf("the motor will move\n");
+      printf("id0.state = 2.2 to start\n");
+    break;
+
+    case 23:
+      printf("conf0.psi = %f # append to config\n", PIN(psi));
+      printf("done\n");
+
+      PIN(state) = 2.4;
+    break;
+  }
+ 
+}
+
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  //struct iddc_ctx_t * ctx = (struct iddc_ctx_t *)ctx_ptr;
+  struct iddc_pin_ctx_t *pins = (struct iddc_pin_ctx_t *)pin_ptr;
+
+  if(PIN(en) <= 0.0){
+    PIN(state) = 0.0;
+  }
+
+  switch((int)(PIN(state) * 10.0 + 0.5)){
+    case 0:
+      PIN(en_out) = 0.0;
+      PIN(cmd_mode) = 0.0;
+      PIN(timer) = 0.0;
+      PIN(d_cmd) = 0.0;
+      PIN(q_cmd) = 0.0;
+      PIN(cur_bw) = 1.0;
+
+      if(PIN(en) > 0.0){
+        PIN(state) = 1.0;
+      }
+    break;
+
+    case 12: // r
+      PIN(en_out) = 1.0;
+      PIN(cmd_mode) = 1.0;
+      PIN(cur_bw) = 1.0;
+      PIN(q_cmd) = 0.0;
+      PIN(com_pos) = 0.0;
+
+      PIN(d_cmd) = PIN(test_cur);
+
+      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_out) = 0.0;
+        PIN(tmp0) = 0.0;
+        PIN(tmp1) = 0.0;
+
+        PIN(avg_test_volt) = PIN(test_cur) * MAX(PIN(r), 0.01);
+        PIN(avg_test_volt) = LIMIT(PIN(avg_test_volt), PIN(pwm_volt) / 2.0);
+      }
+    break;
+
+    case 13: // l
+      PIN(en_out) = 1.0;
+      PIN(cmd_mode) = 0.0;
+      PIN(q_cmd) = 0.0;
+      PIN(cur_bw) = 1.0;
+      
+      //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) < PIN(avg_test_volt)){
+        PIN(tmp0) = PIN(tmp0) * 0.99 + PIN(id_fb) * 0.01;
+        PIN(tmp1) = PIN(tmp1) * 0.99 + PIN(ud_fb) * 0.01;
+        PIN(d_cmd) = PIN(avg_test_volt) * 1.5;
+      }
+      else{
+        PIN(tmp2) = PIN(tmp2) * 0.99 + PIN(id_fb) * 0.01;
+        PIN(tmp3) = PIN(tmp3) * 0.99 + PIN(ud_fb) * 0.01;
+        PIN(d_cmd) = PIN(avg_test_volt) * 0.5;
+      }
+
+      PIN(timer) += period;
+      if(PIN(timer) >= 1.0){
+        PIN(l) = ABS(PIN(tmp1) - PIN(tmp3)) / ABS(PIN(tmp0) - PIN(tmp2)) * period;
+        PIN(timer) = 0.0;
+        PIN(state) = 1.4;
+        PIN(d_cmd) = PIN(avg_test_volt);
+        PIN(en_out) = 0.0;
+        // PIN(tmp0) = 0.0;
+        // PIN(tmp1) = 0.0;
+      }
+    break;
+
+    case 22: // psi
+      PIN(en_out) = 1.0;
+      PIN(cmd_mode) = 1.0;
+      PIN(cur_bw) = 250.0;
+      PIN(d_cmd) = 0.0;
+
+      float vel_error = PIN(test_vel) - PIN(vel_fb);
+      PIN(cur_sum) += PIN(ki) * vel_error * period;
+
+      PIN(q_cmd) = PIN(vel_bw) * period * vel_error + PIN(cur_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_out) = 0.0;
+        PIN(d_cmd) = 0.0;
+        PIN(q_cmd) = 0.0;
+        PIN(cur_sum) = 0.0;
+        PIN(cmd_mode) = 0.0;
+
+        PIN(state) = 2.3;
+      }
+    break;
+  }
+}
+
+
+hal_comp_t iddc_comp_struct = {
+    .name      = "iddc",
+    .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 iddc_ctx_t),
+    .pin_count = sizeof(struct iddc_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
+};

shared/comps/idm.c

@@ -90,18 +90,18 @@ static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
 
     case 13:
       if(PIN(sys) > 0.0){
-        printf("conf0.j_sys = %f\n", PIN(inertia));
+        printf("conf0.j_sys = %f # append to config\n", PIN(inertia));
       }
       else{
-        printf("conf0.j = %f\n", PIN(inertia));
+        printf("conf0.j = %f # append to config\n", PIN(inertia));
       }
-      printf("conf0.d = %f\n", PIN(damping));
+      printf("conf0.d = %f # append to config\n", PIN(damping));
-      printf("conf0.f = %f\n", PIN(friction));
+      printf("conf0.f = %f # append to config\n", PIN(friction));
-      printf("pid0.l = %f\n", PIN(load));
+      printf("done\n");
+      //printf("pid0.l = %f\n", PIN(load));
       PIN(state) = 1.4;
     break;
   }
- 
 }
 
 static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {

shared/comps/idpmsm.c

@@ -1,9 +1,9 @@
-#include "idp_comp.h"
+#include "idpmsm_comp.h"
 #include "hal.h"
 #include "defines.h"
 #include "angle.h"
 
-HAL_COMP(idp);
+HAL_COMP(idpmsm);
 
 HAL_PIN(d_cmd);
 HAL_PIN(q_cmd);
@@ -51,8 +51,8 @@ HAL_PIN(tmp3);
 HAL_PIN(avg_test_volt);
 
 static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
-  //struct idp_ctx_t * ctx = (struct idp_ctx_t *)ctx_ptr;
+  //struct idpmsm_ctx_t * ctx = (struct idpmsm_ctx_t *)ctx_ptr;
-  struct idp_pin_ctx_t *pins = (struct idp_pin_ctx_t *)pin_ptr;
+  struct idpmsm_pin_ctx_t *pins = (struct idpmsm_pin_ctx_t *)pin_ptr;
   PIN(test_cur) = 4.0;
   PIN(test_vel) = 50.0;
   PIN(ki) = 1.0;
@@ -63,8 +63,8 @@ static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
 }
 
 static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
-  //struct idp_ctx_t * ctx = (struct idp_ctx_t *)ctx_ptr;
+  //struct idpmsm_ctx_t * ctx = (struct idpmsm_ctx_t *)ctx_ptr;
-  struct idp_pin_ctx_t *pins = (struct idp_pin_ctx_t *)pin_ptr;
+  struct idpmsm_pin_ctx_t *pins = (struct idpmsm_pin_ctx_t *)pin_ptr;
 
   switch((int)(PIN(state) * 10.0 + 0.5)){
     case 0:
@@ -96,8 +96,8 @@ static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
     break;
 
     case 14:
-      printf("conf0.r = %f\n", PIN(r));
+      printf("conf0.r = %f # append to config\n", PIN(r));
-      printf("conf0.l = %f\n", PIN(l));
+      printf("conf0.l = %f # append to config\n", PIN(l));
       PIN(state) = 2.0;
     break;
 
@@ -120,10 +120,10 @@ static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
     break;
 
     case 25: // pp, out_rev, com_offset
-      printf("conf0.polecount = %f\n", PIN(pp));
+      printf("conf0.polecount = %f # append to config\n", PIN(pp));
-      printf("conf0.mot_fb_offset = %f\n", PIN(com_offset));
+      printf("conf0.mot_fb_offset = %f # append to config\n", PIN(com_offset));
       if(PIN(out_rev) > 0.0){
-        printf("conf0.out_rev = 1\n");
+        printf("conf0.out_rev = 1 # append to config\n");
       }
       PIN(state) = 3.0;
     break;
@@ -148,7 +148,8 @@ static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
     break;
 
     case 33:
-      printf("conf0.psi = %f\n", PIN(psi));
+      printf("conf0.psi = %f # append to config\n", PIN(psi));
+      printf("done\n");
 
       PIN(state) = 3.4;
     break;
@@ -157,8 +158,8 @@ static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
 }
 
 static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
-  //struct idp_ctx_t * ctx = (struct idp_ctx_t *)ctx_ptr;
+  //struct idpmsm_ctx_t * ctx = (struct idpmsm_ctx_t *)ctx_ptr;
-  struct idp_pin_ctx_t *pins = (struct idp_pin_ctx_t *)pin_ptr;
+  struct idpmsm_pin_ctx_t *pins = (struct idpmsm_pin_ctx_t *)pin_ptr;
 
   if(PIN(en) <= 0.0){
     PIN(state) = 0.0;
@@ -272,7 +273,7 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
       PIN(d_cmd) = LIMIT(PIN(d_cmd), PIN(pwm_volt));
       PIN(com_pos) = 0.0;
 
-      PIN(com_offset) = PIN(com_offset) * 0.99 + -PIN(pos_fb) * 0.01;
+      PIN(com_offset) = PIN(com_offset) * 0.95 - PIN(pos_fb) * 0.05;
 
       PIN(timer) += period;
       if(PIN(timer) >= 2.0){
@@ -321,8 +322,8 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
 }
 
 
-hal_comp_t idp_comp_struct = {
+hal_comp_t idpmsm_comp_struct = {
-    .name      = "idp",
+    .name      = "idpmsm",
     .nrt       = nrt,
     .rt        = rt_func,
     .frt       = 0,
@@ -331,6 +332,6 @@ hal_comp_t idp_comp_struct = {
     .frt_start = 0,
     .rt_stop   = 0,
     .frt_stop  = 0,
-    .ctx_size  = 0,//sizeof(struct idp_ctx_t),
+    .ctx_size  = 0,//sizeof(struct idpmsm_ctx_t),
-    .pin_count = sizeof(struct idp_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
+    .pin_count = sizeof(struct idpmsm_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
 };

shared/comps/ids.c

@@ -0,0 +1,245 @@
+#include "ids_comp.h"
+#include "hal.h"
+#include "defines.h"
+#include "angle.h"
+
+HAL_COMP(ids);
+
+HAL_PIN(en);
+
+HAL_PIN(state);
+HAL_PIN(param);
+HAL_PIN(step);
+HAL_PIN(rep);
+
+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(pos_cmd);
+HAL_PIN(vel_cmd);
+HAL_PIN(acc_cmd);
+
+HAL_PIN(pos_error);
+HAL_PIN(vel_error);
+
+HAL_PIN(pos_bw);
+HAL_PIN(vel_bw);
+HAL_PIN(vel_d);
+HAL_PIN(cur_bw);
+
+HAL_PIN(ff);
+HAL_PIN(kp);
+HAL_PIN(ks);
+HAL_PIN(kv);
+HAL_PIN(kt);
+HAL_PIN(kd);
+
+HAL_PINA(params, 3);
+HAL_PINA(max_params, 3);
+
+HAL_PIN(target);
+HAL_PIN(cost);
+HAL_PIN(min_cost);
+HAL_PIN(auto_step);
+
+HAL_PIN(timer);
+
+static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  //struct ids_ctx_t * ctx = (struct ids_ctx_t *)ctx_ptr;
+  struct ids_pin_ctx_t *pins = (struct ids_pin_ctx_t *)pin_ptr;
+  PIN(pos_bw) = 10.0;
+  PIN(vel_bw) = 100.0;
+  PIN(vel_d) = 10.0;
+  PINA(params, 0) = PIN(vel_bw);
+  PINA(params, 1) = 1.0 / PIN(vel_d);
+  PINA(params, 2) = PIN(pos_bw);
+
+  PIN(kp) = 1.0;
+  PIN(ks) = 0.0;
+  PIN(kv) = 1.0;
+  PIN(kt) = 1.2;
+  PIN(kd) = 0.7;
+
+  PIN(ff) = 1.0;
+
+  PIN(max_vel) = 100.0;
+  PIN(max_acc) = 1000.0;
+  PIN(min_pos) = -10.0;
+  PIN(max_pos) = 10.0;
+
+  PIN(param) = 0.0;
+  PIN(step) = 0.25;
+  PIN(rep) = 2.0;
+
+  PIN(auto_step) = 1.0;
+}
+
+static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  //struct ids_ctx_t * ctx = (struct ids_ctx_t *)ctx_ptr;
+  struct ids_pin_ctx_t *pins = (struct ids_pin_ctx_t *)pin_ptr;
+
+  switch((int)(PIN(state) * 10.0 + 0.5)){
+    case 0:
+    break;
+    
+    case 10:
+      PIN(state) = 1.1;
+      PIN(target) = PIN(max_pos);
+
+      if(PIN(auto_step) >= 1){
+        PIN(state) = 1.2;
+      }
+      else{
+        printf("Tune PPI bandwidth and damping\n");
+        printf("the motor will move\n");
+        printf("ids0.state = 1.2 to start\n");
+      }
+    break;
+
+    case 13:
+      printf("conf0.pos_bw = %f # append to config\n", PIN(pos_bw));
+      printf("conf0.vel_bw = %f # append to config\n", PIN(vel_bw));
+      printf("conf0.vel_d = %f # append to config\n", PIN(vel_d));
+      printf("done\n");
+      PIN(state) = 1.4;
+    break;
+  }
+}
+
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  //struct ids_ctx_t * ctx = (struct ids_ctx_t *)ctx_ptr;
+  struct ids_pin_ctx_t *pins = (struct ids_pin_ctx_t *)pin_ptr;
+
+  if(PIN(en) <= 0.0){
+    PIN(state) = 0.0;
+  }
+
+  switch((int)(PIN(state) * 10.0 + 0.5)){
+    case 0:
+      PIN(acc_cmd) = 0.0;
+      PIN(vel_cmd) = 0.0;
+      PIN(acc) = 0.0;
+      PIN(vel) = 0.0;
+
+      PIN(param) = 0.0;
+      PIN(step) = 0.1;
+      PIN(rep) = 1.0;
+
+      PIN(pos_bw) = 10.0;
+      PIN(vel_bw) = 100.0;
+      PIN(vel_d) = 10.0;
+      PINA(params, 0) = PIN(vel_bw);
+      PINA(params, 1) = 1.0 / PIN(vel_d);
+      PINA(params, 2) = PIN(pos_bw);
+
+      PINA(max_params, 0) = PIN(cur_bw) / 2.0;
+      PINA(max_params, 1) = 1.0;
+
+      PIN(min_cost) = (PIN(max_pos) - PIN(min_pos)) * (PIN(max_pos) - PIN(min_pos)) / PIN(max_vel) * 100.0;
+      PIN(cost) = PIN(min_cost);
+
+      if(PIN(en) > 0.0){
+        PIN(state) = 1.0;
+      }
+    break;
+
+    case 12:
+      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;
+
+      if(time_to_go < period / 2.0){
+        time_to_go = 0.0;
+        to_go = 0.0;
+        vel = 0.0;
+        acc = 0.0;
+        PIN(pos) = PIN(target);
+        PIN(vel) = 0.0;
+        PIN(acc) = 0.0;
+      }
+
+      PIN(acc) = LIMIT(acc, PIN(max_acc));
+
+      PIN(pos_cmd) = mod(PIN(pos));
+      PIN(vel_cmd) = PIN(vel) * PIN(ff);
+      PIN(acc_cmd) = PIN(acc) * PIN(ff);
+      
+      PIN(cost) += ABS(PIN(pos_error)) * PIN(kp) * period;
+      PIN(cost) += PIN(pos_error) * PIN(pos_error) * PIN(ks) * period;
+      PIN(cost) += PIN(vel_error) * PIN(vel_error) * PIN(kv) * period;
+      
+
+      PIN(timer) += period;
+      if(PIN(timer) < (ABS(PIN(max_pos) - PIN(min_pos)) / PIN(max_vel) + 2.0 * PIN(max_vel) / PIN(max_acc))){
+        PIN(target) = PIN(max_pos);
+      }
+      else{
+        PIN(target) = PIN(min_pos);
+      }
+      if(PIN(timer) > 2.0 * (ABS(PIN(max_pos) - PIN(min_pos)) / PIN(max_vel) + 2.0 * PIN(max_vel) / PIN(max_acc))){
+        PIN(timer) = 0.0;
+      }
+      
+      if(PIN(timer) == 0.0){
+        PIN(min_cost) = MIN(PIN(cost), PIN(min_cost));
+
+        PINA(max_params, 2) = PINA(params, 0) / 2.0;
+
+        if(PINA(params, (int)PIN(param)) > PINA(max_params, (int)PIN(param))){
+          PINA(params, (int)PIN(param)) = PINA(max_params, (int)PIN(param));
+          PIN(min_cost) = PIN(cost) * 10.0;
+          PIN(param)++;
+        }
+        else if(PIN(cost) > PIN(min_cost) * PIN(kt)){
+          PINA(params, (unsigned int)PIN(param)) *= PIN(kd);
+          PIN(min_cost) = PIN(cost) * 10.0;
+          PIN(param)++;
+        }
+        else{
+          PINA(params, (unsigned int)PIN(param)) *= 1.0 + PIN(step);
+        }
+
+        PIN(cost) = 0.0;
+      }
+      
+      PIN(pos_bw) = PINA(params, 2);
+      PIN(vel_bw) = PINA(params, 0);
+      PIN(vel_d) = 1.0 / PINA(params, 1);
+      
+      if(PIN(param) > 2.0){
+        PIN(acc_cmd) = 0.0;
+        PIN(vel_cmd) = 0.0;
+        PIN(param) = 0.0;
+
+        PIN(state) = 1.3;
+      }
+    break;
+  }
+}
+
+
+hal_comp_t ids_comp_struct = {
+    .name      = "ids",
+    .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 ids_ctx_t),
+    .pin_count = sizeof(struct ids_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
+};