pbio/integrator: Simplify position integrator state.

This changes the used integral value to be one-step-ahead instead of
returning the previous error. This makes little difference in the overal
output (it marginally changes the proportional term), but makes it
easier to adjust the range limits on the current error instead of the
previous error. That is, the position error and the target error are
now from the same time stamp.

Also removes one state value which was no longer used.

Author: laurensvalk

lib/pbio/include/pbio/integrator.h

@@ -36,9 +36,7 @@ typedef struct _pbio_position_integrator_t {
     bool trajectory_running; // Whether the trajectory is running (1) or paused (0)
     uint32_t time_pause_begin; // Time at which we began pausing most recently, stopping integration
     uint32_t time_paused_total; // Total time we spent in a paused state
-    int32_t count_err_prev; // Position error in the previous control iteration
     int32_t count_err_integral; // Ongoing integral of position error
-    int32_t count_err_integral_max; // Maximum value of integrator
     pbio_control_settings_t *settings; // Control settings, which includes integrator settings.
 } pbio_position_integrator_t;
 

lib/pbio/src/integrator.c

@@ -143,7 +143,6 @@ void pbio_position_integrator_reset(pbio_position_integrator_t *itg, pbio_contro
     itg->count_err_integral = 0;
     itg->time_paused_total = 0;
     itg->time_pause_begin = time_now;
-    itg->count_err_prev = 0;
     itg->trajectory_running = false;
 
     // Resume integration
@@ -153,8 +152,7 @@ void pbio_position_integrator_reset(pbio_position_integrator_t *itg, pbio_contro
 
 int32_t pbio_position_integrator_update(pbio_position_integrator_t *itg, int32_t position_error, int32_t target_error) {
 
-    // Previous error will be multiplied by time delta and then added to integral (unless we limit growth)
-    int32_t error_now = itg->count_err_prev;
+    int32_t error_now = position_error;
 
     // Check if integrator magnitude would decrease due to this error
     bool decrease = pbio_int_math_abs(itg->count_err_integral + pbio_control_settings_mul_by_loop_time(error_now)) < pbio_int_math_abs(itg->count_err_integral);
@@ -187,9 +185,7 @@ int32_t pbio_position_integrator_update(pbio_position_integrator_t *itg, int32_t
             pbio_control_settings_div_by_gain(itg->settings->actuation_max, itg->settings->pid_ki));
     }
 
-    // Keep the error for use in next update
-    itg->count_err_prev = position_error;
-
+    // Return current value.
     return itg->count_err_integral;
 }