new idm, improve doc, id_sys

conf/template/id_mot.txt

@@ -16,9 +16,8 @@ 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/id_sys.txt

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

conf/template/pid.txt

@@ -95,6 +95,7 @@ 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 move +
+the motor will turn with a max velocity of iddc0.test_vel (rad/s) and apply a max current of iddc0.test_cur (A) +
 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 iddc
+link id_dc
 
 - 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 move +
+the motor will turn with a max velocity of idpmsm0.test_vel (rad/s) and apply a max current of idpmsm0.test_cur (A) +
 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 idpmsm
+link id_pmsm
 
 - reset stmbl by typing
 [source]
@@ -83,20 +83,18 @@ 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 <<mechanical parameters>>
+- go to <<motor mechanical parameters>>
 
-== mechanical parameters
-connect the motor to the load
+== motor mechanical parameters
 
 === manual tuning
 [source]
 conf0.j
-conf0.j_load
 
 === auto tuning
-this takes about 3 minutes +
-connect the motor to the load +
-the motor will move +
+this takes about 2 minutes +
+disconnect the motor from 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
@@ -105,7 +103,7 @@ link pid
 link <your_motor_type>
 link <your_feedback_type>
 link misc
-link idm
+link id_mot
 <your_electrical_parameters>
 
 - reset stmbl by typing
@@ -117,6 +115,41 @@ 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
@@ -129,7 +162,7 @@ conf0.vel_d
 === auto tuning
 this takes about 2 minutes +
 connect the motor to the load +
-the motor will move +
+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) +
 follow the green instructions in Servoterm
 
 - set your config to
@@ -138,7 +171,7 @@ link pid
 link <your_motor_type>
 link <your_feedback_type>
 link misc
-link ids
+link id_pid
 <your_electrical_parameters>
 <your_mechanical_parameters>
 

shared/comps/iddc.c

@@ -108,6 +108,7 @@ 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,7 +8,11 @@ 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);
@@ -22,16 +26,19 @@ 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);
@@ -50,12 +57,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) = 10.0;
-  PIN(ji) = 200.0;
-  PIN(di) = 10.0;
+  PIN(li) = 0.01;
+  PIN(di) = 1.0;
+  PIN(ji) = 10.0;
   PIN(pos_bw) = 5.0;
   PIN(vel_bw) = 40.0;
-  PIN(vel_d) = 1.0;
+  PIN(vel_d) = 2.0;
   PIN(max_vel) = 25.0;
   PIN(max_acc) = 250.0;
   PIN(min_pos) = -10.0;
@@ -88,9 +95,10 @@ static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
       }
     break;
 
-    case 13:
+    case 14:
       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));
@@ -98,8 +106,13 @@ 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");
-      //printf("pid0.l = %f\n", PIN(load));
-      PIN(state) = 1.4;
+      if(PIN(sys) > 0.0){
+        printf("continue with id_pid\n");
+      }
+      else{
+        printf("continue with id_sys or id_pid\n");
+      }
+      PIN(state) = 1.5;
     break;
   }
 }
@@ -112,6 +125,13 @@ 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;
@@ -121,42 +141,178 @@ 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: // 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)));
+    case 12: 
+      max_acc = PIN(max_acc) / 5.0 * PIN(sub_state);
+      max_vel = PIN(max_vel) / 5.0 * PIN(sub_state);
 
-        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(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) / 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(pos) += PIN(vel_cmd) * period + PIN(acc_cmd) * period * period / 2.0;
-        PIN(vel_cmd) += PIN(acc_cmd) * 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_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(time) += period;
+        // PIN(friction_sum) += SIGN(PIN(vel_cmd)) * PIN(fb_torque) * period;
+        // PIN(inertia_sum) += PIN(acc_cmd) * PIN(fb_torque) * period;
+        // PIN(damping_sum) += PIN(vel_cmd) * PIN(fb_torque) * period;
+        // PIN(load_sum) += PIN(fb_torque) * period;
       }
 
-      PIN(pos_cmd) = mod(PIN(pos));
+      if(ABS(PIN(acc_cmd)) > 0.0){
+        PIN(acc_time) += period;
+        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);
+        }
+
+        if(PIN(vel_time) > period){
+          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;
@@ -170,13 +326,13 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
       
 
       PIN(timer) += period;
-      if(PIN(timer) > 180.0){
+      if(PIN(timer) > 90.0){
         PIN(timer) = 0.0;
         PIN(acc_cmd) = 0.0;
         PIN(vel_cmd) = 0.0;
         PIN(amp) = 0.0;
 
-        PIN(state) = 1.3;
+        PIN(state) = 1.4;
       }
     break;
   }

shared/comps/idpmsm.c

@@ -150,6 +150,7 @@ 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;