pbio/drivebase: Make trajectory synchronization easier to follow.

Also avoid direct access to integrator state.

Author: laurensvalk

lib/pbio/include/pbio/integrator.h

@@ -47,6 +47,7 @@ typedef struct _pbio_position_integrator_t {
 
 uint32_t pbio_position_integrator_get_ref_time(pbio_position_integrator_t *itg, uint32_t time_now);
 void pbio_position_integrator_pause(pbio_position_integrator_t *itg, uint32_t time_now);
+bool pbio_position_integrator_is_paused(pbio_position_integrator_t *itg);
 void pbio_position_integrator_resume(pbio_position_integrator_t *itg, uint32_t time_now);
 void pbio_position_integrator_reset(pbio_position_integrator_t *itg, pbio_control_settings_t *settings, uint32_t time_now);
 int32_t pbio_position_integrator_update(pbio_position_integrator_t *itg, int32_t position_error, int32_t target_error);

lib/pbio/src/drivebase.c

@@ -402,10 +402,14 @@ static pbio_error_t pbio_drivebase_update(pbio_drivebase_t *db) {
 
     // Both controllers are able to stop the other when it stalls. This ensures
     // they complete at exactly the same time.
-    if (pbio_control_type_is_position(&db->control_distance) && !db->control_distance.position_integrator.trajectory_running) {
+    if (pbio_control_type_is_position(&db->control_distance) &&
+        pbio_position_integrator_is_paused(&db->control_distance.position_integrator)) {
+        // If distance controller is paused, pause heading control too.
         pbio_position_integrator_pause(&db->control_heading.position_integrator, time_now);
     }
-    if (pbio_control_type_is_position(&db->control_heading) && !db->control_heading.position_integrator.trajectory_running) {
+    if (pbio_control_type_is_position(&db->control_heading) &&
+        pbio_position_integrator_is_paused(&db->control_heading.position_integrator)) {
+        // If heading controller is paused, pause distance control too.
         pbio_position_integrator_pause(&db->control_distance.position_integrator, time_now);
     }
 

lib/pbio/src/integrator.c

@@ -123,6 +123,10 @@ void pbio_position_integrator_pause(pbio_position_integrator_t *itg, uint32_t ti
     itg->time_pause_begin = time_now;
 }
 
+bool pbio_position_integrator_is_paused(pbio_position_integrator_t *itg) {
+    return !itg->trajectory_running;
+}
+
 void pbio_position_integrator_resume(pbio_position_integrator_t *itg, uint32_t time_now) {
 
     // Return if already trajectory_running