pbio/servo_settings: Move friction torque reduction.

The friction torque value was previously reduced because friction compensation caused too much overshoot. But the model isn't the right place to do this.

This restores the original values and moves the factor 2 reduction to wherever the friction value is used. Now, we can decide for each case if it makes sense to keep this reduction.

Author: laurensvalk

lib/pbio/doc/control/motor_model.py

@@ -190,7 +190,7 @@ def make_model(name, *, V, tau_0, tau_x, w_0, w_x, i_0, i_x, a, Lm, h, gain):
             .d_torque_d_voltage = {round(PRESCALE_VOLTAGE / dtau_dv.subs(model).evalf())},
             .d_torque_d_speed = {round(PRESCALE_SPEED / dtau_dw.subs(model).evalf())},
             .d_torque_d_acceleration = {round(PRESCALE_ACCELERATION / dtau_da.subs(model).evalf())},
-            .torque_friction = {round(tau_s * c_tau / 2)},
+            .torque_friction = {round(tau_s * c_tau)},
             .gain = {gain},
         }};"""
     )

lib/pbio/src/motor/servo_settings.c

@@ -30,7 +30,7 @@ static const pbio_observer_model_t model_technic_s_angular = {
     .d_torque_d_voltage = 17203,
     .d_torque_d_speed = 12282,
     .d_torque_d_acceleration = 354592,
-    .torque_friction = 4591,
+    .torque_friction = 9182,
     .gain = 500,
 };
 
@@ -51,7 +51,7 @@ static const pbio_observer_model_t model_technic_m_angular = {
     .d_torque_d_voltage = 8071,
     .d_torque_d_speed = 5903,
     .d_torque_d_acceleration = 163151,
-    .torque_friction = 10707,
+    .torque_friction = 21413,
     .gain = 2000,
 };
 
@@ -72,7 +72,7 @@ static const pbio_observer_model_t model_technic_l_angular = {
     .d_torque_d_voltage = 2872,
     .d_torque_d_speed = 1919,
     .d_torque_d_acceleration = 40344,
-    .torque_friction = 11620,
+    .torque_friction = 23239,
     .gain = 4000,
 };
 
@@ -93,7 +93,7 @@ static const pbio_observer_model_t model_interactive = {
     .d_torque_d_voltage = 11923,
     .d_torque_d_speed = 10599,
     .d_torque_d_acceleration = 207820,
-    .torque_friction = 5613,
+    .torque_friction = 11227,
     .gain = 2000,
 };
 
@@ -114,7 +114,7 @@ static const pbio_observer_model_t model_technic_l = {
     .d_torque_d_voltage = 6110,
     .d_torque_d_speed = 6837,
     .d_torque_d_acceleration = 108520,
-    .torque_friction = 13215,
+    .torque_friction = 26430,
     .gain = 1500,
 };
 
@@ -135,7 +135,7 @@ static const pbio_observer_model_t model_technic_xl = {
     .d_torque_d_voltage = 6908,
     .d_torque_d_speed = 7713,
     .d_torque_d_acceleration = 116867,
-    .torque_friction = 6446,
+    .torque_friction = 12893,
     .gain = 2000,
 };
 
@@ -158,7 +158,7 @@ static const pbio_observer_model_t model_movehub = {
     .d_torque_d_voltage = 8438,
     .d_torque_d_speed = 10851,
     .d_torque_d_acceleration = 155017,
-    .torque_friction = 12417,
+    .torque_friction = 24835,
     .gain = 2000,
 };
 
@@ -185,7 +185,7 @@ static const pbio_observer_model_t model_ev3_l = {
     .d_torque_d_voltage = 3587,
     .d_torque_d_speed = 2083,
     .d_torque_d_acceleration = 19838,
-    .torque_friction = 8238,
+    .torque_friction = 16476,
     .gain = 4000,
 };
 
@@ -206,7 +206,7 @@ static const pbio_observer_model_t model_ev3_m = {
     .d_torque_d_voltage = 8051,
     .d_torque_d_speed = 7365,
     .d_torque_d_acceleration = 94428,
-    .torque_friction = 9159,
+    .torque_friction = 18317,
     .gain = 2000,
 };
 

lib/pbio/src/observer.c

@@ -92,7 +92,7 @@ static void update_stall_state(pbio_observer_t *obs, uint32_t time, pbio_dcmotor
         // fully stuck (where applied voltage equals feedback).
         -feedback_voltage > (voltage * 3) / 4 &&
         // Feedback voltage is nonnegligible, i.e. larger than friction torque.
-        voltage > 5 * pbio_observer_torque_to_voltage(obs->model, obs->model->torque_friction)
+        voltage > 5 * pbio_observer_torque_to_voltage(obs->model, obs->model->torque_friction / 2)
         ) {
         // If this is the rising edge of the stall flag, reset start time.
         if (!obs->stalled) {
@@ -157,7 +157,7 @@ void pbio_observer_update(pbio_observer_t *obs, uint32_t time, const pbio_angle_
     voltage += feedback_voltage;
 
     // The only modeled torque is a static friction torque.
-    int32_t torque = obs->speed > 0 ? m->torque_friction: -m->torque_friction;
+    int32_t torque = obs->speed > 0 ? m->torque_friction / 2: -m->torque_friction / 2;
 
     // Get next state based on current state and input: x(k+1) = Ax(k) + Bu(k)
     pbio_angle_add_mdeg(&obs->angle,
@@ -207,7 +207,7 @@ bool pbio_observer_is_stalled(const pbio_observer_t *obs, uint32_t time, uint32_
 
 int32_t pbio_observer_get_feedforward_torque(const pbio_observer_model_t *model, int32_t rate_ref, int32_t acceleration_ref) {
 
-    int32_t friction_compensation_torque = model->torque_friction * pbio_int_math_sign(rate_ref);
+    int32_t friction_compensation_torque = model->torque_friction / 2 * pbio_int_math_sign(rate_ref);
     int32_t back_emf_compensation_torque = PRESCALE_SPEED * pbio_int_math_clamp(rate_ref, MAX_NUM_SPEED) / model->d_torque_d_speed;
     int32_t acceleration_torque = PRESCALE_ACCELERATION * pbio_int_math_clamp(acceleration_ref, MAX_NUM_ACCELERATION) / model->d_torque_d_acceleration;