diff --git a/.vscode/launch.json b/.vscode/launch.json index 9f6d330c2..e7f279345 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -105,7 +105,7 @@ "request": "launch", "program": "${workspaceFolder}/bricks/virtualhub/build-debug/virtualhub-micropython", "args": [ - "${workspaceFolder}/tests/virtualhub/motor/car.py" + "${workspaceFolder}/tests/virtualhub/motor/drivebase.py" ], "stopAtEntry": false, "cwd": "${workspaceFolder}", diff --git a/lib/pbio/include/pbio/drivebase.h b/lib/pbio/include/pbio/drivebase.h index 9d72f85a7..e0d5f2237 100644 --- a/lib/pbio/include/pbio/drivebase.h +++ b/lib/pbio/include/pbio/drivebase.h @@ -29,7 +29,23 @@ typedef struct _pbio_drivebase_t { bool control_paused; pbio_servo_t *left; pbio_servo_t *right; + /** + * Offset of the reported heading angle with respect to the measured value. + * + * Not used if gyro control is active. Then the gyro module handles offset. + */ + pbio_angle_t heading_offset; + /** + * Heading controller. + */ pbio_control_t control_heading; + /** + * Offset of the reported distance with respect to the measured value. + */ + pbio_angle_t distance_offset; + /** + * Distance controller. + */ pbio_control_t control_distance; } pbio_drivebase_t; @@ -56,6 +72,7 @@ pbio_error_t pbio_drivebase_stop(pbio_drivebase_t *db, pbio_control_on_completio // Measuring and settings: pbio_error_t pbio_drivebase_get_state_user(pbio_drivebase_t *db, int32_t *distance, int32_t *drive_speed, int32_t *angle, int32_t *turn_rate); +pbio_error_t pbio_drivebase_reset(pbio_drivebase_t *db, int32_t distance, int32_t angle); pbio_error_t pbio_drivebase_get_drive_settings(const pbio_drivebase_t *db, int32_t *drive_speed, int32_t *drive_acceleration, int32_t *drive_deceleration, int32_t *turn_rate, int32_t *turn_acceleration, int32_t *turn_deceleration); pbio_error_t pbio_drivebase_set_drive_settings(pbio_drivebase_t *db, int32_t drive_speed, int32_t drive_acceleration, int32_t drive_deceleration, int32_t turn_rate, int32_t turn_acceleration, int32_t turn_deceleration); pbio_error_t pbio_drivebase_set_use_gyro(pbio_drivebase_t *db, bool use_gyro); diff --git a/lib/pbio/src/drivebase.c b/lib/pbio/src/drivebase.c index ae1d94a17..bc92a5b52 100644 --- a/lib/pbio/src/drivebase.c +++ b/lib/pbio/src/drivebase.c @@ -21,7 +21,7 @@ static pbio_drivebase_t drivebases[PBIO_CONFIG_NUM_DRIVEBASES]; /** * Gets the state of the drivebase update loop. * - * This becomes true after a successful call to pbio_drivebase_setup and + * This becomes true after a successful call to pbio_drivebase_get_drivebase and * becomes false when there is an error. Such as when the cable is unplugged. * * @param [in] db The drivebase instance @@ -117,11 +117,11 @@ static void drivebase_adopt_settings(pbio_control_settings_t *s_distance, pbio_c * Get the physical and estimated state of a drivebase in units of control. * * @param [in] db The drivebase instance - * @param [out] state_distance Physical and estimated state of the distance. - * @param [out] state_heading Physical and estimated state of the heading. + * @param [out] state_distance Physical and estimated state of the distance based on motor angles. + * @param [out] state_heading Physical and estimated state of the heading based on motor angles. * @return Error code. */ -static pbio_error_t pbio_drivebase_get_state_control(pbio_drivebase_t *db, pbio_control_state_t *state_distance, pbio_control_state_t *state_heading) { +static pbio_error_t pbio_drivebase_get_state_via_motors(pbio_drivebase_t *db, pbio_control_state_t *state_distance, pbio_control_state_t *state_heading) { // Get left servo state pbio_control_state_t state_left; @@ -150,7 +150,35 @@ static pbio_error_t pbio_drivebase_get_state_control(pbio_drivebase_t *db, pbio_ state_heading->speed_estimate = state_distance->speed_estimate - state_right.speed_estimate; state_heading->speed = state_distance->speed - state_right.speed; + return PBIO_SUCCESS; +} + +/** + * Get the physical and estimated state of a drivebase in units of control. + * + * @param [in] db The drivebase instance + * @param [out] state_distance Physical and estimated state of the distance. + * @param [out] state_heading Physical and estimated state of the heading. + * @return Error code. + */ +static pbio_error_t pbio_drivebase_get_state_control(pbio_drivebase_t *db, pbio_control_state_t *state_distance, pbio_control_state_t *state_heading) { + + // Gets the "measured" state according to the driver motors. + pbio_error_t err = pbio_drivebase_get_state_via_motors(db, state_distance, state_heading); + if (err != PBIO_SUCCESS) { + return err; + } + + // Subtract distance offset. + pbio_angle_diff(&state_distance->position, &db->distance_offset, &state_distance->position); + pbio_angle_diff(&state_distance->position_estimate, &db->distance_offset, &state_distance->position_estimate); + + // Subtract heading offset + pbio_angle_diff(&state_heading->position, &db->heading_offset, &state_heading->position); + pbio_angle_diff(&state_heading->position_estimate, &db->heading_offset, &state_heading->position_estimate); + // Optionally use gyro to override the heading source for more accuracy. + // The gyro manages its own offset, so we don't need to subtract it here. if (db->use_gyro) { pbio_imu_get_heading_scaled(&state_heading->position, &state_heading->speed, db->control_heading.settings.ctl_steps_per_app_step); } @@ -235,7 +263,7 @@ static pbio_error_t pbio_drivebase_stop_from_servo(void *drivebase, bool clear_p #define ROT_MDEG_OVER_PI (114592) // 360 000 / pi /** - * Gets drivebase instance from two servo instances. + * Gets and sets up drivebase instance from two servo instances. * * @param [out] db_address Drivebase instance if available. * @param [in] left Left servo instance. @@ -329,6 +357,10 @@ pbio_error_t pbio_drivebase_get_drivebase(pbio_drivebase_t **db_address, pbio_se return PBIO_ERROR_INVALID_ARG; } + // Reset offsets so current distance and angle are 0. This relies on the + // geometry, so it is done after the scaling is set. + pbio_drivebase_reset(db, 0, 0); + // Finish setup. By default, don't use gyro. return pbio_drivebase_set_use_gyro(db, false); } @@ -687,6 +719,50 @@ pbio_error_t pbio_drivebase_get_state_user(pbio_drivebase_t *db, int32_t *distan return PBIO_SUCCESS; } +/** + * Stops the drivebase and resets the accumulated drivebase state in user units. + * + * If the gyro is being used for control, it will be reset to the same angle. + * + * @param [in] db The drivebase instance. + * @param [out] distance Distance traveled in mm. + * @param [out] angle Angle turned in degrees. + * @return Error code. + */ +pbio_error_t pbio_drivebase_reset(pbio_drivebase_t *db, int32_t distance, int32_t angle) { + + // Physically stops motors and stops the ongoing controllers, simplifying + // the state reset since we won't need to restart ongoing motion. + pbio_error_t err = pbio_drivebase_stop(db, PBIO_CONTROL_ON_COMPLETION_COAST); + if (err != PBIO_SUCCESS) { + return err; + } + + // Get measured state according to motor encoders. + pbio_control_state_t measured_distance; + pbio_control_state_t measured_heading; + err = pbio_drivebase_get_state_via_motors(db, &measured_distance, &measured_heading); + if (err != PBIO_SUCCESS) { + return err; + } + + // We want to get: reported_new = measured - offset_new + // So we can do: offset_new = measured - reported_new + pbio_angle_t reported_new; + + pbio_angle_from_low_res(&reported_new, distance, db->control_distance.settings.ctl_steps_per_app_step); + pbio_angle_diff(&measured_distance.position, &reported_new, &db->distance_offset); + + pbio_angle_from_low_res(&reported_new, angle, db->control_heading.settings.ctl_steps_per_app_step); + pbio_angle_diff(&measured_heading.position, &reported_new, &db->heading_offset); + + // Whether or not the gyro is being used, synchronize heading and drivebase + // angle state. + pbio_imu_set_heading(angle); + + return PBIO_SUCCESS; +} + /** * Gets the drivebase settings in user units. diff --git a/pybricks/robotics/pb_type_drivebase.c b/pybricks/robotics/pb_type_drivebase.c index d035f8063..c573e818a 100644 --- a/pybricks/robotics/pb_type_drivebase.c +++ b/pybricks/robotics/pb_type_drivebase.c @@ -29,8 +29,6 @@ typedef struct _pb_type_DriveBase_obj_t pb_type_DriveBase_obj_t; struct _pb_type_DriveBase_obj_t { mp_obj_base_t base; pbio_drivebase_t *db; - int32_t initial_distance; - int32_t initial_heading; #if PYBRICKS_PY_COMMON_CONTROL mp_obj_t heading_control; mp_obj_t distance_control; @@ -39,18 +37,18 @@ struct _pb_type_DriveBase_obj_t { }; // pybricks.robotics.DriveBase.reset -static mp_obj_t pb_type_DriveBase_reset(mp_obj_t self_in) { - pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in); +static mp_obj_t pb_type_DriveBase_reset(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { - int32_t distance, drive_speed, angle, turn_rate; - pb_assert(pbio_drivebase_get_state_user(self->db, &distance, &drive_speed, &angle, &turn_rate)); + PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, + pb_type_DriveBase_obj_t, self, + PB_ARG_DEFAULT_INT(distance, 0), + PB_ARG_DEFAULT_INT(angle, 0)); - self->initial_distance = distance; - self->initial_heading = angle; + pb_assert(pbio_drivebase_reset(self->db, pb_obj_get_int(distance_in), pb_obj_get_int(angle_in))); return mp_const_none; } -MP_DEFINE_CONST_FUN_OBJ_1(pb_type_DriveBase_reset_obj, pb_type_DriveBase_reset); +static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_reset_obj, 1, pb_type_DriveBase_reset); // pybricks.robotics.DriveBase.__init__ static mp_obj_t pb_type_DriveBase_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) { @@ -80,9 +78,6 @@ static mp_obj_t pb_type_DriveBase_make_new(const mp_obj_type_t *type, size_t n_a self->distance_control = pb_type_Control_obj_make_new(&self->db->control_distance); #endif - // Reset drivebase state - pb_type_DriveBase_reset(MP_OBJ_FROM_PTR(self)); - // List of awaitables associated with this drivebase. By keeping track, // we can cancel them as needed when a new movement is started. self->awaitables = mp_obj_new_list(0, NULL); @@ -243,7 +238,7 @@ static mp_obj_t pb_type_DriveBase_distance(mp_obj_t self_in) { int32_t distance, _; pb_assert(pbio_drivebase_get_state_user(self->db, &distance, &_, &_, &_)); - return mp_obj_new_int(distance - self->initial_distance); + return mp_obj_new_int(distance); } MP_DEFINE_CONST_FUN_OBJ_1(pb_type_DriveBase_distance_obj, pb_type_DriveBase_distance); @@ -254,7 +249,7 @@ static mp_obj_t pb_type_DriveBase_angle(mp_obj_t self_in) { int32_t heading, _; pb_assert(pbio_drivebase_get_state_user(self->db, &_, &_, &heading, &_)); - return mp_obj_new_int(heading - self->initial_heading); + return mp_obj_new_int(heading); } MP_DEFINE_CONST_FUN_OBJ_1(pb_type_DriveBase_angle_obj, pb_type_DriveBase_angle); @@ -266,9 +261,9 @@ static mp_obj_t pb_type_DriveBase_state(mp_obj_t self_in) { pb_assert(pbio_drivebase_get_state_user(self->db, &distance, &drive_speed, &heading, &turn_rate)); mp_obj_t ret[4]; - ret[0] = mp_obj_new_int(distance - self->initial_distance); + ret[0] = mp_obj_new_int(distance); ret[1] = mp_obj_new_int(drive_speed); - ret[2] = mp_obj_new_int(heading - self->initial_heading); + ret[2] = mp_obj_new_int(heading); ret[3] = mp_obj_new_int(turn_rate); return mp_obj_new_tuple(4, ret); diff --git a/tests/virtualhub/motor/drivebase.py b/tests/virtualhub/motor/drivebase.py index 1a6efd823..00183af36 100644 --- a/tests/virtualhub/motor/drivebase.py +++ b/tests/virtualhub/motor/drivebase.py @@ -2,19 +2,67 @@ from pybricks.tools import wait from pybricks.parameters import Port, Direction from pybricks.robotics import DriveBase +from umath import pi # Initialize default "Driving Base" with medium motors and wheels. left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) right_motor = Motor(Port.B) drive_base = DriveBase(left_motor, right_motor, wheel_diameter=56, axle_track=112) -drive_base.settings( - straight_speed=500, straight_acceleration=1000, turn_rate=500, turn_acceleration=2000 -) + +def expect_state( + expected_distance, expected_drive_speed, expected_angle, expected_angular_velocity +): + expected_state = ( + expected_distance, + expected_drive_speed, + expected_angle, + expected_angular_velocity, + ) + distance, drive_speed, angle, angular_velocity = drive_base.state() + if ( + abs(expected_distance - distance) > 10 + or abs(expected_drive_speed - drive_speed) > 30 + or abs(expected_angle - angle) > 10 + or abs(expected_angular_velocity - angular_velocity) > 30 + ): + raise ValueError("Expected {0} but got {1}".format(expected_state, drive_base.state())) + + +# Expect zeroed state on startup. +expect_state(0, 0, 0, 0) # Drive straight forward and back again. drive_base.straight(500) +expect_state(500, 0, 0, 0) drive_base.straight(-500) +expect_state(0, 0, 0, 0) + +# Curve and back again. +drive_base.curve(100, 90) +expect_state(pi / 2 * 100, 0, 90, 0) +drive_base.curve(-100, 90) +expect_state(0, 0, 0, 0) -wait(100) -drive_base.stop() +# Test setting of distance and angle state. +drive_base.reset(1000, 360) +expect_state(1000, 0, 360, 0) +drive_base.straight(-1000) +drive_base.turn(-360) +expect_state(0, 0, 0, 0) + +# Expect to reach target speed. +drive_base.drive(200, 25) +wait(500) +expect_state(drive_base.distance(), 200, drive_base.angle(), 25) + +# Expect reset to stop the robot +drive_base.reset() +wait(500) +expect_state(drive_base.distance(), 0, drive_base.angle(), 0) + +# Drive fast. +drive_base.settings( + straight_speed=500, straight_acceleration=1000, turn_rate=500, turn_acceleration=2000 +) +drive_base.straight(500)