Makefile

@@ -121,10 +121,8 @@ 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/idpmsm.c
+SHARED_COMPS += shared/comps/idp.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 = 25
+conf0.pos_bw = 10
-conf0.vel_bw = 250
+conf0.vel_bw = 500
 conf0.vel_d = 5
 conf0.vel_g = 0.1
 conf0.torque_g = 1
-conf0.cur_bw = 1000
+conf0.cur_bw = 1500
 conf0.max_sat = 0.2

conf/template/dc.txt

@@ -1,7 +1,18 @@
+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_ttc0.torque = pid0.torque_cmd
+dc_limits0.ac_volt = hv0.pwm_volt
+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

@@ -1,28 +0,0 @@
-
-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

@@ -0,0 +1,27 @@
+
+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

@@ -1,28 +0,0 @@
-
-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,37 +1,9 @@
 
-load ids
+link idm
-ids0.rt_prio = 4
 
-ids0.en = fault0.en_out
+idm0.sys = 1
-pid0.pos_ext_cmd = ids0.pos_cmd
+pid0.j_mot = conf0.j
-pid0.vel_ext_cmd = ids0.vel_cmd
+pid0.j_sys = idm0.inertia
-pid0.acc_ext_cmd = ids0.acc_cmd
+pid0.d = idm0.damping
-
+pid0.f = idm0.friction
-pid0.pos_bw = ids0.pos_bw
+pid0.l = idm0.load
-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

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

docs/src/feedback.adoc

@@ -1,36 +0,0 @@
-: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

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

docs/src/pinout.adoc

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

docs/src/servoterm.adoc

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

docs/src/supply.adoc

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

docs/src/tuning.adoc

@@ -1,155 +0,0 @@
-: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.001);
+  float psi_m  = MAX(PIN(psi), 0.01);
   float torque = PIN(torque);
 
   PIN(cur) = torque / psi_m;

shared/comps/fault.c

@@ -96,19 +96,6 @@ 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

@@ -1,245 +0,0 @@
-#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 # append to config\n", PIN(inertia));
+        printf("conf0.j_sys = %f\n", PIN(inertia));
       }
       else{
-        printf("conf0.j = %f # append to config\n", PIN(inertia));
+        printf("conf0.j = %f\n", PIN(inertia));
       }
-      printf("conf0.d = %f # append to config\n", PIN(damping));
+      printf("conf0.d = %f\n", PIN(damping));
-      printf("conf0.f = %f # append to config\n", PIN(friction));
+      printf("conf0.f = %f\n", PIN(friction));
-      printf("done\n");
+      printf("pid0.l = %f\n", PIN(load));
-      //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/idp.c

@@ -1,9 +1,9 @@
-#include "idpmsm_comp.h"
+#include "idp_comp.h"
 #include "hal.h"
 #include "defines.h"
 #include "angle.h"
 
-HAL_COMP(idpmsm);
+HAL_COMP(idp);
 
 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 idpmsm_ctx_t * ctx = (struct idpmsm_ctx_t *)ctx_ptr;
+  //struct idp_ctx_t * ctx = (struct idp_ctx_t *)ctx_ptr;
-  struct idpmsm_pin_ctx_t *pins = (struct idpmsm_pin_ctx_t *)pin_ptr;
+  struct idp_pin_ctx_t *pins = (struct idp_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 idpmsm_ctx_t * ctx = (struct idpmsm_ctx_t *)ctx_ptr;
+  //struct idp_ctx_t * ctx = (struct idp_ctx_t *)ctx_ptr;
-  struct idpmsm_pin_ctx_t *pins = (struct idpmsm_pin_ctx_t *)pin_ptr;
+  struct idp_pin_ctx_t *pins = (struct idp_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 # append to config\n", PIN(r));
+      printf("conf0.r = %f\n", PIN(r));
-      printf("conf0.l = %f # append to config\n", PIN(l));
+      printf("conf0.l = %f\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 # append to config\n", PIN(pp));
+      printf("conf0.polecount = %f\n", PIN(pp));
-      printf("conf0.mot_fb_offset = %f # append to config\n", PIN(com_offset));
+      printf("conf0.mot_fb_offset = %f\n", PIN(com_offset));
       if(PIN(out_rev) > 0.0){
-        printf("conf0.out_rev = 1 # append to config\n");
+        printf("conf0.out_rev = 1\n");
       }
       PIN(state) = 3.0;
     break;
@@ -148,8 +148,7 @@ static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
     break;
 
     case 33:
-      printf("conf0.psi = %f # append to config\n", PIN(psi));
+      printf("conf0.psi = %f\n", PIN(psi));
-      printf("done\n");
 
       PIN(state) = 3.4;
     break;
@@ -158,8 +157,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 idpmsm_ctx_t * ctx = (struct idpmsm_ctx_t *)ctx_ptr;
+  //struct idp_ctx_t * ctx = (struct idp_ctx_t *)ctx_ptr;
-  struct idpmsm_pin_ctx_t *pins = (struct idpmsm_pin_ctx_t *)pin_ptr;
+  struct idp_pin_ctx_t *pins = (struct idp_pin_ctx_t *)pin_ptr;
 
   if(PIN(en) <= 0.0){
     PIN(state) = 0.0;
@@ -273,7 +272,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.95 - PIN(pos_fb) * 0.05;
+      PIN(com_offset) = PIN(com_offset) * 0.99 + -PIN(pos_fb) * 0.01;
 
       PIN(timer) += period;
       if(PIN(timer) >= 2.0){
@@ -322,8 +321,8 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
 }
 
 
-hal_comp_t idpmsm_comp_struct = {
+hal_comp_t idp_comp_struct = {
-    .name      = "idpmsm",
+    .name      = "idp",
     .nrt       = nrt,
     .rt        = rt_func,
     .frt       = 0,
@@ -332,6 +331,6 @@ hal_comp_t idpmsm_comp_struct = {
     .frt_start = 0,
     .rt_stop   = 0,
     .frt_stop  = 0,
-    .ctx_size  = 0,//sizeof(struct idpmsm_ctx_t),
+    .ctx_size  = 0,//sizeof(struct idp_ctx_t),
-    .pin_count = sizeof(struct idpmsm_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
+    .pin_count = sizeof(struct idp_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
 };

shared/comps/ids.c

@@ -1,245 +0,0 @@
-#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),
-};