From 9b84f4f79e23e293ef9953322fa659c6fce8e652 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 9 Nov 2020 16:44:13 +0100 Subject: [PATCH] pbio/control: log control instead of servo and drivebase Attach logger to control object instead of to the servo and the drivebase separately. This makes it possible to log additional control data and reduces duplicate code. Also reduces move hub firmware size by several hundred bytes. --- lib/pbio/include/pbio/control.h | 2 + lib/pbio/include/pbio/drivebase.h | 1 - lib/pbio/include/pbio/logger.h | 2 +- lib/pbio/include/pbio/servo.h | 1 - lib/pbio/src/control.c | 12 ++++++ lib/pbio/src/drivebase.c | 60 +++----------------------- lib/pbio/src/servo.c | 61 ++------------------------- lib/pbio/test/src/servo.c | 10 ++--- pybricks/common.h | 1 - pybricks/common/pb_type_control.c | 6 +++ pybricks/common/pb_type_motor.c | 4 -- pybricks/robotics/pb_type_drivebase.c | 5 --- 12 files changed, 37 insertions(+), 128 deletions(-) diff --git a/lib/pbio/include/pbio/control.h b/lib/pbio/include/pbio/control.h index ad2e4405a..37de9549e 100644 --- a/lib/pbio/include/pbio/control.h +++ b/lib/pbio/include/pbio/control.h @@ -13,6 +13,7 @@ #include #include #include +#include #include @@ -73,6 +74,7 @@ typedef struct _pbio_control_t { pbio_rate_integrator_t rate_integrator; pbio_count_integrator_t count_integrator; pbio_control_on_target_t on_target_func; + pbio_log_t log; bool stalled; bool on_target; } pbio_control_t; diff --git a/lib/pbio/include/pbio/drivebase.h b/lib/pbio/include/pbio/drivebase.h index 118f654ff..14ef80de2 100644 --- a/lib/pbio/include/pbio/drivebase.h +++ b/lib/pbio/include/pbio/drivebase.h @@ -13,7 +13,6 @@ typedef struct _pbio_drivebase_t { pbio_servo_t *right; int32_t sum_offset; int32_t dif_offset; - pbio_log_t log; pbio_control_t control_heading; pbio_control_t control_distance; } pbio_drivebase_t; diff --git a/lib/pbio/include/pbio/logger.h b/lib/pbio/include/pbio/logger.h index 2ad94af4a..6647cc6a0 100644 --- a/lib/pbio/include/pbio/logger.h +++ b/lib/pbio/include/pbio/logger.h @@ -13,7 +13,7 @@ #define NUM_DEFAULT_LOG_VALUES (1) // Maximum number of values to be logged per sample -#define MAX_LOG_VALUES (20) +#define MAX_LOG_VALUES (NUM_DEFAULT_LOG_VALUES + 7) typedef struct _pbio_log_t { bool active; diff --git a/lib/pbio/include/pbio/servo.h b/lib/pbio/include/pbio/servo.h index ca4da03e2..2754e8a18 100644 --- a/lib/pbio/include/pbio/servo.h +++ b/lib/pbio/include/pbio/servo.h @@ -31,7 +31,6 @@ typedef struct _pbio_servo_t { pbio_tacho_t *tacho; pbio_control_t control; pbio_port_t port; - pbio_log_t log; } pbio_servo_t; pbio_error_t pbio_servo_setup(pbio_servo_t *srv, pbio_direction_t direction, fix16_t gear_ratio); diff --git a/lib/pbio/src/control.c b/lib/pbio/src/control.c index 80c360091..5b3f4570b 100644 --- a/lib/pbio/src/control.c +++ b/lib/pbio/src/control.c @@ -106,6 +106,18 @@ void control_update(pbio_control_t *ctl, int32_t time_now, int32_t count_now, in *actuation_type = PBIO_ACTUATION_DUTY; *control = duty; } + + // Log control data + int32_t log_data[] = { + (time_ref - ctl->trajectory.t0) / 1000, + count_now, + rate_now, + PBIO_ACTUATION_DUTY, + *control, + count_ref, + rate_ref, + }; + pbio_logger_update(&ctl->log, log_data); } diff --git a/lib/pbio/src/drivebase.c b/lib/pbio/src/drivebase.c index 3a2a2df79..54a89123d 100644 --- a/lib/pbio/src/drivebase.c +++ b/lib/pbio/src/drivebase.c @@ -8,8 +8,6 @@ #include #include -#define DRIVEBASE_LOG_NUM_VALUES (15 + NUM_DEFAULT_LOG_VALUES) - #if PBDRV_CONFIG_NUM_MOTOR_CONTROLLER != 0 static pbio_error_t drivebase_adopt_settings(pbio_control_settings_t *s_distance, pbio_control_settings_t *s_heading, pbio_control_settings_t *s_left, pbio_control_settings_t *s_right) { @@ -142,44 +140,6 @@ static pbio_error_t pbio_drivebase_actuate(pbio_drivebase_t *db, pbio_actuation_ return err; } -// Log motor data for a motor that is being actively controlled -static pbio_error_t drivebase_log_update(pbio_drivebase_t *db, - int32_t time_now, - int32_t sum, - int32_t sum_rate, - int32_t sum_control, - int32_t dif, - int32_t dif_rate, - int32_t dif_control) { - - int32_t buf[DRIVEBASE_LOG_NUM_VALUES]; - buf[0] = time_now; - buf[1] = sum; - buf[2] = sum_rate; - buf[3] = sum_control; - buf[4] = dif; - buf[5] = dif_rate; - buf[6] = dif_control; - - int32_t sum_ref, sum_ref_ext, sum_rate_ref, sum_rate_err, sum_rate_err_integral, sum_acceleration_ref; - pbio_trajectory_get_reference(&db->control_distance.trajectory, time_now, &sum_ref, &sum_ref_ext, &sum_rate_ref, &sum_acceleration_ref); - pbio_rate_integrator_get_errors(&db->control_distance.rate_integrator, time_now, sum_rate_ref, sum, sum_ref, &sum_rate_err, &sum_rate_err_integral); - buf[7] = sum_ref; - buf[8] = sum_rate_err_integral; - buf[9] = sum_rate_ref; - buf[10] = sum_rate_err_integral; - - int32_t dif_ref, dif_ref_ext, dif_rate_ref, dif_rate_err, dif_rate_err_integral, dif_acceleration_ref; - pbio_trajectory_get_reference(&db->control_heading.trajectory, time_now, &dif_ref, &dif_ref_ext, &dif_rate_ref, &dif_acceleration_ref); - pbio_rate_integrator_get_errors(&db->control_heading.rate_integrator, time_now, dif_rate_ref, dif, dif_ref, &dif_rate_err, &dif_rate_err_integral); - buf[11] = dif_ref; - buf[12] = dif_rate_err_integral; - buf[13] = dif_rate_ref; - buf[14] = dif_rate_err_integral; - - return pbio_logger_update(&db->log, buf); -} - pbio_error_t pbio_drivebase_setup(pbio_drivebase_t *db, pbio_servo_t *left, pbio_servo_t *right, fix16_t wheel_diameter, fix16_t axle_track) { pbio_error_t err; @@ -214,9 +174,6 @@ pbio_error_t pbio_drivebase_setup(pbio_drivebase_t *db, pbio_servo_t *left, pbio db->right = right; pbio_drivebase_claim_servos(db, false); - // Initialize log - db->log.num_values = DRIVEBASE_LOG_NUM_VALUES; - // Adopt settings as the average or sum of both servos, except scaling err = drivebase_adopt_settings(&db->control_distance.settings, &db->control_heading.settings, &db->left->control.settings, &db->right->control.settings); if (err != PBIO_SUCCESS) { @@ -310,6 +267,12 @@ pbio_error_t pbio_drivebase_stop_force(pbio_drivebase_t *db) { } pbio_error_t pbio_drivebase_update(pbio_drivebase_t *db) { + + // If passive, log and exit + if (db->control_heading.type == PBIO_CONTROL_NONE || db->control_distance.type == PBIO_CONTROL_NONE) { + return PBIO_SUCCESS; + } + // Get the physical state int32_t time_now, sum, sum_rate, dif, dif_rate; pbio_error_t err = drivebase_get_state(db, &time_now, &sum, &sum_rate, &dif, &dif_rate); @@ -317,11 +280,6 @@ pbio_error_t pbio_drivebase_update(pbio_drivebase_t *db) { return err; } - // If passive, log and exit - if (db->control_heading.type == PBIO_CONTROL_NONE || db->control_distance.type == PBIO_CONTROL_NONE) { - return drivebase_log_update(db, time_now, sum, sum_rate, 0, dif, dif_rate, 0); - } - // Get control signals int32_t sum_control, dif_control; pbio_actuation_t sum_actuation, dif_actuation; @@ -334,11 +292,7 @@ pbio_error_t pbio_drivebase_update(pbio_drivebase_t *db) { } // Actuate - err = pbio_drivebase_actuate(db, sum_actuation, sum_control, dif_control); - if (err != PBIO_SUCCESS) { - return err; - } - return drivebase_log_update(db, time_now, sum, sum_rate, sum_control, dif, dif_rate, dif_control); + return pbio_drivebase_actuate(db, sum_actuation, sum_control, dif_control); } pbio_error_t pbio_drivebase_straight(pbio_drivebase_t *db, int32_t distance, int32_t drive_speed, int32_t drive_acceleration) { diff --git a/lib/pbio/src/servo.c b/lib/pbio/src/servo.c index a8f7504e4..654955107 100644 --- a/lib/pbio/src/servo.c +++ b/lib/pbio/src/servo.c @@ -11,12 +11,9 @@ #include #include #include -#include #if PBDRV_CONFIG_NUM_MOTOR_CONTROLLER != 0 -#define SERVO_LOG_NUM_VALUES (9 + NUM_DEFAULT_LOG_VALUES) - // TODO: Move to config and enable only known motors for platform static pbio_control_settings_t settings_servo_ev3_medium = { .max_rate = 2000, @@ -184,9 +181,6 @@ pbio_error_t pbio_servo_setup(pbio_servo_t *srv, pbio_direction_t direction, fix // For a servo, counts per output unit is counts per degree at the gear train output srv->control.settings.counts_per_unit = fix16_mul(F16C(PBDRV_CONFIG_COUNTER_COUNTS_PER_DEGREE, 0), gear_ratio); - // Configure the logs for a servo - srv->log.num_values = SERVO_LOG_NUM_VALUES; - return PBIO_SUCCESS; } @@ -270,48 +264,6 @@ static pbio_error_t pbio_servo_actuate(pbio_servo_t *srv, pbio_actuation_t actua return PBIO_SUCCESS; } -// Log motor data for a motor that is being actively controlled -static pbio_error_t pbio_servo_log_update(pbio_servo_t *srv, int32_t time_now, int32_t count_now, int32_t rate_now, pbio_actuation_t actuation, int32_t control) { - - int32_t buf[SERVO_LOG_NUM_VALUES]; - memset(buf, 0, sizeof(buf)); - - // Log the physical state of the motor - buf[1] = count_now; - buf[2] = rate_now; - - // Log the applied control signal - buf[3] = actuation; - buf[4] = control; - - // If control is active, log additional data about the maneuver - if (srv->control.type != PBIO_CONTROL_NONE) { - - // Get the time of reference evaluation - int32_t time_ref = pbio_control_get_ref_time(&srv->control, time_now); - - // Log the time since start of control trajectory - buf[0] = (time_ref - srv->control.trajectory.t0) / 1000; - - // Log reference signals. These values are only meaningful for time based commands - int32_t count_ref, count_ref_ext, rate_ref, err, err_integral, acceleration_ref; - pbio_trajectory_get_reference(&srv->control.trajectory, time_ref, &count_ref, &count_ref_ext, &rate_ref, &acceleration_ref); - - if (srv->control.type == PBIO_CONTROL_ANGLE) { - pbio_count_integrator_get_errors(&srv->control.count_integrator, count_now, count_ref, &err, &err_integral); - } else { - pbio_rate_integrator_get_errors(&srv->control.rate_integrator, rate_now, rate_ref, count_now, count_ref, &err, &err_integral); - } - - buf[5] = count_ref; - buf[6] = rate_ref; - buf[7] = err; // count err for angle control, rate err for timed control - buf[8] = err_integral; - } - - return pbio_logger_update(&srv->log, buf); -} - pbio_error_t pbio_servo_control_update(pbio_servo_t *srv) { // Read the physical state @@ -335,20 +287,15 @@ pbio_error_t pbio_servo_control_update(pbio_servo_t *srv) { if (err != PBIO_SUCCESS) { return err; } - return pbio_servo_log_update(srv, time_now, count_now, rate_now, state, control); + // Log control data + int32_t log_data[] = {time_now, count_now, rate_now, state, control}; + return pbio_logger_update(&srv->control.log, log_data); } - // Calculate control signal control_update(&srv->control, time_now, count_now, rate_now, &actuation, &control); // Apply the control type and signal - err = pbio_servo_actuate(srv, actuation, control); - if (err != PBIO_SUCCESS) { - return err; - } - - // Log data if logger enabled - return pbio_servo_log_update(srv, time_now, count_now, rate_now, actuation, control); + return pbio_servo_actuate(srv, actuation, control); } /* pbio user functions */ diff --git a/lib/pbio/test/src/servo.c b/lib/pbio/test/src/servo.c index 1ecc8cd41..2c52b006d 100644 --- a/lib/pbio/test/src/servo.c +++ b/lib/pbio/test/src/servo.c @@ -89,12 +89,12 @@ static PT_THREAD(test_servo_run_func(struct pt *pt, const char *name, pbio_error tt_uint_op(pbio_motorpoll_set_servo_status(servo, PBIO_ERROR_AGAIN), ==, PBIO_SUCCESS); // only logging one row since we read it after every iteration - log_buf = malloc(sizeof(*log_buf) * pbio_logger_cols(&servo->log)); + log_buf = malloc(sizeof(*log_buf) * pbio_logger_cols(&servo->control.log)); if (log_buf == NULL) { tt_abort_perror("failed to allocate log_buf"); } - pbio_logger_start(&servo->log, log_buf, 1, 1); + pbio_logger_start(&servo->control.log, log_buf, 1, 1); tt_uint_op(func(servo), ==, PBIO_SUCCESS); @@ -253,17 +253,17 @@ static PT_THREAD(test_servo_run_func(struct pt *pt, const char *name, pbio_error } // write current state to log file - if (pbio_logger_rows(&servo->log)) { + if (pbio_logger_rows(&servo->control.log)) { fprintf(log_file, "%d,", clock_to_msec(clock_time())); fprintf(log_file, "%d,", test_motor_driver.output); fprintf(log_file, "%d,", test_motor_driver.duty_cycle); - for (int i = 0; i < pbio_logger_cols(&servo->log); i++) { + for (int i = 0; i < pbio_logger_cols(&servo->control.log); i++) { fprintf(log_file, "%d,", log_buf[i]); } fprintf(log_file, "\n"); - servo->log.sampled = 0; + servo->control.log.sampled = 0; } clock_tick(1); diff --git a/pybricks/common.h b/pybricks/common.h index 3665620a0..36a4f1abc 100644 --- a/pybricks/common.h +++ b/pybricks/common.h @@ -54,7 +54,6 @@ mp_obj_t logger_obj_make_new(pbio_log_t *log); typedef struct _common_Motor_obj_t { mp_obj_base_t base; pbio_servo_t *srv; - mp_obj_t logger; mp_obj_t control; } common_Motor_obj_t; diff --git a/pybricks/common/pb_type_control.c b/pybricks/common/pb_type_control.c index 168f4bf1a..a7b0581e5 100644 --- a/pybricks/common/pb_type_control.c +++ b/pybricks/common/pb_type_control.c @@ -20,6 +20,7 @@ typedef struct _common_Control_obj_t { mp_obj_base_t base; pbio_control_t *control; mp_obj_t scale; + mp_obj_t logger; } common_Control_obj_t; // pybricks._common.Control.__init__/__new__ @@ -30,6 +31,10 @@ mp_obj_t common_Control_obj_make_new(pbio_control_t *control) { self->control = control; + // Create an instance of the Logger class + self->logger = logger_obj_make_new(&self->control->log); + self->control->log.num_values = MAX_LOG_VALUES; + #if MICROPY_PY_BUILTINS_FLOAT self->scale = mp_obj_new_float(fix16_to_float(control->settings.counts_per_unit)); #else @@ -246,6 +251,7 @@ STATIC const mp_rom_map_elem_t common_Control_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_done), MP_ROM_PTR(&common_Control_done_obj) }, { MP_ROM_QSTR(MP_QSTR_stalled), MP_ROM_PTR(&common_Control_stalled_obj) }, { MP_ROM_QSTR(MP_QSTR_scale), MP_ROM_ATTRIBUTE_OFFSET(common_Control_obj_t, scale) }, + { MP_ROM_QSTR(MP_QSTR_log), MP_ROM_ATTRIBUTE_OFFSET(common_Control_obj_t, logger) }, }; STATIC MP_DEFINE_CONST_DICT(common_Control_locals_dict, common_Control_locals_dict_table); diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index bab5253ac..f17ef1684 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -92,9 +92,6 @@ STATIC mp_obj_t common_Motor_make_new(const mp_obj_type_t *type, size_t n_args, self->base.type = (mp_obj_type_t *)type; self->srv = srv; - // Create an instance of the Logger class - self->logger = logger_obj_make_new(&self->srv->log); - // Create an instance of the Control class self->control = common_Control_obj_make_new(&self->srv->control); @@ -336,7 +333,6 @@ STATIC const mp_rom_map_elem_t common_Motor_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_run_angle), MP_ROM_PTR(&common_Motor_run_angle_obj) }, { MP_ROM_QSTR(MP_QSTR_run_target), MP_ROM_PTR(&common_Motor_run_target_obj) }, { MP_ROM_QSTR(MP_QSTR_track_target), MP_ROM_PTR(&common_Motor_track_target_obj) }, - { MP_ROM_QSTR(MP_QSTR_log), MP_ROM_ATTRIBUTE_OFFSET(common_Motor_obj_t, logger) }, { MP_ROM_QSTR(MP_QSTR_control), MP_ROM_ATTRIBUTE_OFFSET(common_Motor_obj_t, control) }, }; MP_DEFINE_CONST_DICT(common_Motor_locals_dict, common_Motor_locals_dict_table); diff --git a/pybricks/robotics/pb_type_drivebase.c b/pybricks/robotics/pb_type_drivebase.c index 9de35a0f3..c3bdc9a42 100644 --- a/pybricks/robotics/pb_type_drivebase.c +++ b/pybricks/robotics/pb_type_drivebase.c @@ -26,7 +26,6 @@ typedef struct _robotics_DriveBase_obj_t { pbio_drivebase_t *db; mp_obj_t left; mp_obj_t right; - mp_obj_t logger; mp_obj_t heading_control; mp_obj_t distance_control; int32_t straight_speed; @@ -64,9 +63,6 @@ STATIC mp_obj_t robotics_DriveBase_make_new(const mp_obj_type_t *type, size_t n_ pb_assert(pbio_drivebase_setup(self->db, srv_left, srv_right, pb_obj_get_fix16(wheel_diameter_in), pb_obj_get_fix16(axle_track_in))); pb_assert(pbio_motorpoll_set_drivebase_status(self->db, PBIO_ERROR_AGAIN)); - // Create an instance of the Logger class - self->logger = logger_obj_make_new(&self->db->log); - // Create instances of the Control class self->heading_control = common_Control_obj_make_new(&self->db->control_heading); self->distance_control = common_Control_obj_make_new(&self->db->control_distance); @@ -254,7 +250,6 @@ STATIC const mp_rom_map_elem_t robotics_DriveBase_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_settings), MP_ROM_PTR(&robotics_DriveBase_settings_obj) }, { MP_ROM_QSTR(MP_QSTR_left), MP_ROM_ATTRIBUTE_OFFSET(robotics_DriveBase_obj_t, left) }, { MP_ROM_QSTR(MP_QSTR_right), MP_ROM_ATTRIBUTE_OFFSET(robotics_DriveBase_obj_t, right) }, - { MP_ROM_QSTR(MP_QSTR_log), MP_ROM_ATTRIBUTE_OFFSET(robotics_DriveBase_obj_t, logger) }, { MP_ROM_QSTR(MP_QSTR_heading_control), MP_ROM_ATTRIBUTE_OFFSET(robotics_DriveBase_obj_t, heading_control) }, { MP_ROM_QSTR(MP_QSTR_distance_control), MP_ROM_ATTRIBUTE_OFFSET(robotics_DriveBase_obj_t, distance_control)}, };