conf/template/id_sys.txt

@@ -1,5 +0,0 @@
-
-link id_mot
-pid0.j_mot = conf0.j
-pid0.j_sys = idm0.inertia
-idm0.sys = 1

conf/template/idm.txt

@@ -16,8 +16,9 @@ pid0.pos_bw = idm0.pos_bw
 pid0.vel_bw = idm0.vel_bw
 pid0.vel_d = idm0.vel_d
 
-idm0.torque = pid0.torque_cmd
 idm0.fb_torque = pid0.fb_torque_cmd
+idm0.pos_error = pid0.pos_error
+idm0.vel_error = pid0.vel_error
 
 conf0.max_pos_error = 0
 conf0.max_sat = 10

conf/template/pid.txt

@@ -95,7 +95,6 @@ vel2.torque = pid0.torque_cmd
 pid0.en = fault0.en_pid
 pid0.pos_ext_cmd = reslimit0.pos_out
 pid0.vel_ext_cmd = vel0.vel
-pid0.acc_ext_cmd = vel0.acc
 pid0.pos_fb = fb_switch0.pos_fb
 pid0.vel_fb = vel1.vel
 rev0.in_d = vel0.vel

docs/src/tuning.adoc

@@ -20,7 +20,7 @@ conf0.mot_fb_rev
 ==== auto tuning
 this takes about 20 seconds +
 disconnect the motor from the load +
-the motor will turn with a max velocity of iddc0.test_vel (rad/s) and apply a max current of iddc0.test_cur (A) +
+the motor will move +
 follow the green instructions in Servoterm
 
 - set your config to
@@ -29,7 +29,7 @@ link pid
 link dc
 link <your_feedback_type>
 link misc
-link id_dc
+link iddc
 
 - reset stmbl by typing
 [source]
@@ -63,7 +63,7 @@ conf0.psi // torque constant [Nm/A] or [V/rad/s]
 ==== auto tuning
 this takes about 20 seconds +
 disconnect the motor from the load +
-the motor will turn with a max velocity of idpmsm0.test_vel (rad/s) and apply a max current of idpmsm0.test_cur (A) +
+the motor will move +
 follow the green instructions in Servoterm
 
 - set your config to
@@ -72,7 +72,7 @@ link pid
 link pmsm
 link <your_feedback_type>
 link misc
-link id_pmsm
+link idpmsm
 
 - reset stmbl by typing
 [source]
@@ -83,18 +83,20 @@ reset
 fault0.en = 1
 
 - append the result (conf0.r, conf0.l, conf0.psi, conf0.polecount, conf0.mot_fb_offset, opt. conf0.out_rev) to your config
-- go to <<motor mechanical parameters>>
+- go to <<mechanical parameters>>
 
-== motor mechanical parameters
+== mechanical parameters
+connect the motor to the load
 
 === manual tuning
 [source]
 conf0.j
+conf0.j_load
 
 === auto tuning
-this takes about 2 minutes +
+this takes about 3 minutes +
-disconnect the motor from the load +
+connect the motor to the load +
-the motor will move between idm0.min_pos (rad) and idm0.max_pos (rad) with a max velocity of idm0.max_vel (rad/s) and a max acceleration of idm0.max_acc (rad/s^2) +
+the motor will move +
 follow the green instructions in Servoterm
 
 - set your config to
@@ -103,7 +105,7 @@ link pid
 link <your_motor_type>
 link <your_feedback_type>
 link misc
-link id_mot
+link idm
 <your_electrical_parameters>
 
 - reset stmbl by typing
@@ -115,41 +117,6 @@ reset
 fault0.en = 1
 
 - append the result (conf0.j, conf0.d, conf0.f) to your config
-- go to <<system mechanical parameters>>
-
-== system mechanical parameters
-you can skip this step if you want to tune the motor without load +
-
-=== manual tuning
-[source]
-conf0.j_sys
-conf0.j_lpf
-
-=== auto tuning
-this takes about 2 minutes +
-connect the motor to the load +
-the motor will move between idm0.min_pos (rad) and idm0.max_pos (rad) with a max velocity of idm0.max_vel (rad/s) and a max acceleration of idm0.max_acc (rad/s^2) +
-follow the green instructions in Servoterm
-
-- set your config to
-[source]
-link pid
-link <your_motor_type>
-link <your_feedback_type>
-link misc
-link id_sys
-<your_electrical_parameters>
-<your_mechanical_parameters>
-
-- reset stmbl by typing
-[source]
-reset
-
-- start the tuning by typing
-[source]
-fault0.en = 1
-
-- append the result (conf0.j_sys, conf0.d, conf0.f) to your config
 - go to <<control loop tuning>>
 
 == control loop tuning
@@ -162,7 +129,7 @@ conf0.vel_d
 === auto tuning
 this takes about 2 minutes +
 connect the motor to the load +
-the motor will move between ids0.min_pos (rad) and ids0.max_pos (rad) with a max velocity of ids0.max_vel (rad/s) and a max acceleration of ids0.max_acc (rad/s^2) +
+the motor will move +
 follow the green instructions in Servoterm
 
 - set your config to
@@ -171,7 +138,7 @@ link pid
 link <your_motor_type>
 link <your_feedback_type>
 link misc
-link id_pid
+link ids
 <your_electrical_parameters>
 <your_mechanical_parameters>
 

shared/comps/iddc.c

@@ -108,7 +108,6 @@ static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
         printf("conf0.out_rev = 1 <font color='green'># append to config</font>\n");
       }
       printf("done\n");
-      printf("continue with id_mot\n");
 
       PIN(state) = 2.4;
     break;

shared/comps/idm.c

@@ -8,11 +8,7 @@ HAL_COMP(idm);
 HAL_PIN(en);
 
 HAL_PIN(state);
-HAL_PIN(sub_state);
 HAL_PIN(timer);
-HAL_PIN(acc_time);
-HAL_PIN(vel_time);
-HAL_PIN(time);
 
 HAL_PIN(freq);
 HAL_PIN(amp);
@@ -26,19 +22,16 @@ HAL_PIN(pos_cmd);
 HAL_PIN(vel_cmd);
 HAL_PIN(acc_cmd);
 
+HAL_PIN(pos_error);
+HAL_PIN(vel_error);
+
 HAL_PIN(ji);
 HAL_PIN(fi);
 HAL_PIN(di);
 HAL_PIN(li);
 
-HAL_PIN(torque);
 HAL_PIN(fb_torque);
 
-HAL_PIN(inertia_sum);
-HAL_PIN(friction_sum);
-HAL_PIN(damping_sum);
-HAL_PIN(load_sum);
-
 HAL_PIN(inertia);
 HAL_PIN(damping);
 HAL_PIN(friction);
@@ -57,12 +50,12 @@ static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   //struct idm_ctx_t * ctx = (struct idm_ctx_t *)ctx_ptr;
   struct idm_pin_ctx_t *pins = (struct idm_pin_ctx_t *)pin_ptr;
   PIN(fi) = 0.001;
-  PIN(li) = 0.01;
+  PIN(li) = 10.0;
-  PIN(di) = 1.0;
+  PIN(ji) = 200.0;
-  PIN(ji) = 10.0;
+  PIN(di) = 10.0;
   PIN(pos_bw) = 5.0;
   PIN(vel_bw) = 40.0;
-  PIN(vel_d) = 2.0;
+  PIN(vel_d) = 1.0;
   PIN(max_vel) = 25.0;
   PIN(max_acc) = 250.0;
   PIN(min_pos) = -10.0;
@@ -95,10 +88,9 @@ static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
       }
     break;
 
-    case 14:
+    case 13:
       if(PIN(sys) > 0.0){
         printf("conf0.j_sys = %f <font color='green'># append to config</font>\n", PIN(inertia));
-        //printf("conf0.l = %f <font color='green'># append to config</font>\n", PIN(load));
       }
       else{
         printf("conf0.j = %f <font color='green'># append to config</font>\n", PIN(inertia));
@@ -106,13 +98,8 @@ static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
       printf("conf0.d = %f <font color='green'># append to config</font>\n", PIN(damping));
       printf("conf0.f = %f <font color='green'># append to config</font>\n", PIN(friction));
       printf("done\n");
-      if(PIN(sys) > 0.0){
+      //printf("pid0.l = %f\n", PIN(load));
-        printf("continue with id_pid\n");
+      PIN(state) = 1.4;
-      }
-      else{
-        printf("continue with id_sys or id_pid\n");
-      }
-      PIN(state) = 1.5;
     break;
   }
 }
@@ -125,13 +112,6 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
     PIN(state) = 0.0;
   }
 
-  float to_go;
-  float time_to_go;
-  float acc;
-  float vel;
-  float max_acc;
-  float max_vel;
-
   switch((int)(PIN(state) * 10.0 + 0.5)){
     case 0:
       PIN(timer) = 0.0;
@@ -141,178 +121,42 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
 
       if(PIN(en) > 0.0){
         PIN(state) = 1.0;
-        PIN(sub_state) = 1.0;
-        PIN(timer) = 0.0;
-
-        PIN(inertia_sum) = 0.0;
-        PIN(friction_sum) = 0.0;
-        PIN(damping_sum) = 0.0;
-        PIN(load_sum) = 0.0;
-        PIN(acc_time) = 0.0;
-        PIN(vel_time) = 0.0;
-        PIN(time) = 0.0;
       }
     break;
 
-    case 12: 
+    case 12: // j, d, f
-      max_acc = PIN(max_acc) / 5.0 * PIN(sub_state);
+      if(PIN(timer) < 120.0){
-      max_vel = PIN(max_vel) / 5.0 * PIN(sub_state);
+        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(vel_cmd) * period + PIN(acc_cmd) * period * period / 2.0;
+        float phase = mod(PIN(timer) * 2.0 * M_PI * PIN(freq));
-      PIN(pos_cmd) = mod(PIN(pos));
+        PIN(pos) = PIN(amp) * sinf(phase);
-      PIN(vel_cmd) += PIN(acc_cmd) * period;
+        PIN(vel_cmd) = PIN(amp) * 2.0 * M_PI * PIN(freq) * cosf(phase);
-      to_go = PIN(target) - PIN(pos);
+        PIN(acc_cmd) = -PIN(amp) * 4.0 * M_PI * M_PI * PIN(freq) * PIN(freq) * sinf(phase);
-      time_to_go = sqrtf(2.0 * ABS(to_go) / max_acc);
-      acc = max_acc * SIGN(to_go);
-      vel = acc * time_to_go;
-      vel = LIMIT(vel, max_vel);
-      acc = (vel - PIN(vel_cmd)) / period;
-      PIN(acc_cmd) = LIMIT(acc, max_acc);
 
-
+        PIN(amp) = LIMIT(PIN(amp) + (PIN(max_pos) - PIN(min_pos)) / 2.0 / 30.0 * period, (PIN(max_pos) - PIN(min_pos)) / 2.0);
-      if(time_to_go < period / 2.0){
-        time_to_go = 0.0;
-        to_go = 0.0;
-        PIN(acc_cmd) = 0.0;
-        PIN(pos) = PIN(target);
-        PIN(vel_cmd) = 0.0;
       }
       else{
-        // PIN(time) += period;
+        PIN(pos) += PIN(vel_cmd) * period + PIN(acc_cmd) * period * period / 2.0;
-        // PIN(friction_sum) += SIGN(PIN(vel_cmd)) * PIN(fb_torque) * period;
+        PIN(vel_cmd) += PIN(acc_cmd) * period;
-        // PIN(inertia_sum) += PIN(acc_cmd) * PIN(fb_torque) * period;
+        float to_go = PIN(target) - PIN(pos);
-        // PIN(damping_sum) += PIN(vel_cmd) * PIN(fb_torque) * period;
+        float time_to_go = sqrtf(2.0 * ABS(to_go) / PIN(max_acc));
-        // PIN(load_sum) += PIN(fb_torque) * period;
+        float acc = PIN(max_acc) * SIGN(to_go);
-      }
+        float vel = acc * time_to_go;
+        vel = LIMIT(vel, PIN(max_vel));
+        acc = (vel - PIN(vel_cmd)) / period;
+        PIN(acc_cmd) = LIMIT(acc, PIN(max_acc));
 
-      if(ABS(PIN(acc_cmd)) > 0.0){
+        if(ABS(PIN(max_pos) - PIN(pos)) < 0.1){
-        PIN(acc_time) += period;
+          PIN(target) = PIN(min_pos);
-        PIN(inertia_sum) += PIN(acc_cmd) * PIN(torque) * period;
-      }
-      
-      if(ABS(PIN(vel_cmd)) > 0.0){
-        PIN(vel_time) += period;
-        PIN(damping_sum) += PIN(vel_cmd) * PIN(torque) * period;
-        PIN(friction_sum) += SIGN(PIN(vel_cmd)) * PIN(torque) * period;
-      }
-
-      PIN(time) += period;
-      PIN(load_sum) += PIN(torque) * period;
-      
-
-      PIN(inertia) += period / PIN(ji) * PIN(fb_torque) * PIN(acc_cmd) * period;
-      PIN(damping) += period / PIN(di) * PIN(fb_torque) * PIN(vel_cmd) * period;
-      PIN(friction) += period / PIN(fi) * PIN(fb_torque) * SIGN(PIN(vel_cmd)) * period;
-      PIN(load) += period / PIN(li) * PIN(fb_torque) * period;
-
-      PIN(inertia) = CLAMP(PIN(inertia), 0.000005, 50.0);
-      PIN(damping) = CLAMP(PIN(damping), 0.0, 100.0);
-      PIN(friction) = CLAMP(PIN(friction), 0.0, 100.0);
-      PIN(load) = CLAMP(PIN(load), -100.0, 100.0);
-    
-      PIN(timer) += period;
-      if(PIN(timer) < (ABS(PIN(max_pos) - PIN(min_pos)) / max_vel + 2.0 * max_vel / 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)) / max_vel + 2.0 * max_vel / max_acc)){
-        PIN(timer) = 0.0;
-      }
-
-      if(PIN(timer) == 0.0){
-        PIN(sub_state)++;
-        // if(PIN(acc_time) > period){
-        //   PIN(inertia) += PIN(inertia_sum) / PIN(acc_time);
-        // }
-
-        // if(PIN(vel_time) > period){
-        //   PIN(friction) += PIN(friction_sum) / PIN(vel_time);
-        //   PIN(damping) += PIN(damping_sum) / PIN(vel_time);
-        // }
-
-        // if(PIN(time) > period){
-        //   PIN(load) += PIN(load_sum) / PIN(time);
-        // }
-        // PIN(inertia_sum) /= 0.0;
-        // PIN(friction_sum) = 0.0;
-        // PIN(damping_sum) = 0.0;
-        // PIN(load_sum) = 0.0;
-        // PIN(acc_time) = 0.0;
-        // PIN(vel_time) = 0.0;
-        // PIN(time) = 0.0;
-
-        // PIN(inertia) = CLAMP(PIN(inertia), 0.000005, 50.0);
-        // PIN(damping) = CLAMP(PIN(damping), 0.0, 100.0);
-        // PIN(friction) = CLAMP(PIN(friction), 0.0, 100.0);
-        // PIN(load) = CLAMP(PIN(load), -100.0, 100.0);
-
-        //   PIN(friction) += PIN(friction_sum) / PIN(time);
-        //   PIN(damping) += PIN(damping_sum) / PIN(time);
-        //   PIN(load) += PIN(load_sum) / PIN(time);
-
-        //   PIN(inertia) = CLAMP(PIN(inertia), 0.000005, 50.0);
-        //   PIN(damping) = CLAMP(PIN(damping), 0.0, 100.0);
-        //   PIN(friction) = CLAMP(PIN(friction), 0.0, 100.0);
-        //   PIN(load) = CLAMP(PIN(load), -100.0, 100.0);
-        // }
-        // PIN(time) = 0.0;
-        // PIN(friction_sum) = 0.0;
-        // PIN(inertia_sum) = 0.0;
-        // PIN(damping_sum) = 0.0;
-        // PIN(load_sum) = 0.0;
-      }
-
-      if(PIN(sub_state) > 5.0){
-        PIN(state) = 1.3;
-
-        if(PIN(acc_time) > period){
-          PIN(inertia_sum) /= PIN(acc_time);
         }
-
+        else if(ABS(PIN(min_pos) - PIN(pos)) < 0.1){
-        if(PIN(vel_time) > period){
+          PIN(target) = PIN(max_pos);
-          PIN(friction_sum) /= PIN(vel_time);
-          PIN(damping_sum) /= PIN(vel_time);
-        }
-
-        if(PIN(time) > period){
-          PIN(load_sum) /= PIN(time);
         }
       }
-    break;
 
-    case 13: // j, d, f
-      // if(PIN(timer) < 120.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)));
-
-      //   float phase = mod(PIN(timer) * 2.0 * M_PI * PIN(freq));
-      //   PIN(pos) = PIN(amp) * sinf(phase);
-      //   PIN(vel_cmd) = PIN(amp) * 2.0 * M_PI * PIN(freq) * cosf(phase);
-      //   PIN(acc_cmd) = -PIN(amp) * 4.0 * M_PI * M_PI * PIN(freq) * PIN(freq) * sinf(phase);
-
-      //   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_cmd) * period + PIN(acc_cmd) * period * period / 2.0;
       PIN(pos_cmd) = mod(PIN(pos));
-      PIN(vel_cmd) += PIN(acc_cmd) * period;
-      to_go = PIN(target) - PIN(pos);
-      time_to_go = sqrtf(2.0 * ABS(to_go) / PIN(max_acc));
-      acc = PIN(max_acc) * SIGN(to_go);
-      vel = acc * time_to_go;
-      vel = LIMIT(vel, PIN(max_vel));
-      acc = (vel - PIN(vel_cmd)) / period;
-      PIN(acc_cmd) = 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);
-      }
-      // }
 
       PIN(inertia) += period / PIN(ji) * PIN(fb_torque) * PIN(acc_cmd) * period;
       PIN(damping) += period / PIN(di) * PIN(fb_torque) * PIN(vel_cmd) * period;
@@ -326,13 +170,13 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
       
 
       PIN(timer) += period;
-      if(PIN(timer) > 90.0){
+      if(PIN(timer) > 180.0){
         PIN(timer) = 0.0;
         PIN(acc_cmd) = 0.0;
         PIN(vel_cmd) = 0.0;
         PIN(amp) = 0.0;
 
-        PIN(state) = 1.4;
+        PIN(state) = 1.3;
       }
     break;
   }

shared/comps/idpmsm.c

@@ -150,7 +150,6 @@ static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
     case 33:
       printf("conf0.psi = %f <font color='green'># append to config</font>\n", PIN(psi));
       printf("done\n");
-      printf("continue with id_mot\n");
 
       PIN(state) = 3.4;
     break;