11// SPDX-License-Identifier: MIT
2- // Copyright (c) 2018-2020 The Pybricks Authors
2+ // Copyright (c) 2018-2022 The Pybricks Authors
33
44#include <stdlib.h>
55
1212#if PBIO_CONFIG_NUM_DRIVEBASES > 0
1313
1414// Drivebase objects
15-
1615static pbio_drivebase_t drivebases [PBIO_CONFIG_NUM_DRIVEBASES ];
1716
18- // The drivebase update can run if both servos are successfully updating
17+ /**
18+ * Gets the state of the drivebase update loop.
19+ *
20+ * This becomes true after a successful call to pbio_drivebase_setup and
21+ * becomes false when there is an error. Such as when the cable is unplugged.
22+ *
23+ * @param [in] db The drivebase instance
24+ * @return True if up and running, false if not.
25+ */
1926bool pbio_drivebase_update_loop_is_running (pbio_drivebase_t * db ) {
2027
2128 // Drivebase must have servos.
@@ -32,7 +39,15 @@ bool pbio_drivebase_update_loop_is_running(pbio_drivebase_t *db) {
3239 return pbio_servo_update_loop_is_running (db -> left ) && pbio_servo_update_loop_is_running (db -> right );
3340}
3441
35- 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 ) {
42+ /**
43+ * Sets the drivebase settings based on the left and right motor settings.
44+ *
45+ * @param [out] s_distance Settings of the distance controller.
46+ * @param [out] s_heading Settings of the heading controller.
47+ * @param [in] s_left Settings of the left motor controller.
48+ * @param [in] s_right Settings of the right motor controller.
49+ */
50+ static void 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 ) {
3651
3752 // For all settings, take the value of the least powerful motor to ensure
3853 // that the drivebase can meet the given specs.
@@ -71,7 +86,6 @@ static pbio_error_t drivebase_adopt_settings(pbio_control_settings_t *s_distance
7186 // We make the default turn speed a bit slower. Given the typical wheel
7287 // diameter, the wheels are often quite close together, so this compensates.
7388 s_heading -> speed_default = s_heading -> speed_max / 3 ;
74- return PBIO_SUCCESS ;
7589}
7690
7791// Get the physical and estimated state of a drivebase
@@ -205,10 +219,7 @@ pbio_error_t pbio_drivebase_get_drivebase(pbio_drivebase_t **db_address, pbio_se
205219 }
206220
207221 // Adopt settings as the average or sum of both servos, except scaling
208- err = drivebase_adopt_settings (& db -> control_distance .settings , & db -> control_heading .settings , & left -> control .settings , & right -> control .settings );
209- if (err != PBIO_SUCCESS ) {
210- return err ;
211- }
222+ drivebase_adopt_settings (& db -> control_distance .settings , & db -> control_heading .settings , & left -> control .settings , & right -> control .settings );
212223
213224 // Average rotation of the motors for every 1 degree drivebase rotation.
214225 db -> control_heading .settings .ctl_steps_per_app_step =
@@ -345,13 +356,13 @@ static pbio_error_t pbio_drivebase_drive_relative(pbio_drivebase_t *db, int32_t
345356 return err ;
346357 }
347358
348- // Start controller that controls the sum of both motor counts
359+ // Start controller that controls the average angle of both motors.
349360 err = pbio_control_start_position_control_relative (& db -> control_distance , time_now , & state_distance , distance , drive_speed , on_completion );
350361 if (err != PBIO_SUCCESS ) {
351362 return err ;
352363 }
353364
354- // Start controller that controls the difference between both motor counts
365+ // Start controller that controls half the difference between both angles.
355366 err = pbio_control_start_position_control_relative (& db -> control_heading , time_now , & state_heading , angle , turn_speed , on_completion );
356367 if (err != PBIO_SUCCESS ) {
357368 return err ;
0 commit comments