6675885095

Committed 28 Oct 2023 08:38AM UTC coverage: 56.053% (+10.0%) from 46.074%

Build # 6675885095

Build Type

push

github

Committed by

laurensvalk

Commit Message

pybricks.hubs.MoveHub: Use standard hub init.

The Move Hub cannot have true vector axes, but we can still use this API to initialize the hub using numeric indices.

This ensures we can use the custom orientation not just in tilt, but also in acceleration like we do on other hubs.

Run Details

3616 of 6451 relevant lines covered (56.05%)

20895680.75 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

85.19

/lib/pbio/src/integrator.c

// SPDX-License-Identifier: MIT
// Copyright (c) 2018-2023 The Pybricks Authors

// SPDX-License-Identifier: BSD-3-Clause
// Copyright (c) 2020-2023 LEGO System A/S

#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>

#include <pbio/control_settings.h>
#include <pbio/integrator.h>
#include <pbio/int_math.h>

/**
 * Pauses the speed integrator at the current position error.
 *
 * @param [in]    itg              Speed integrator instance.
 * @param [in]    time_now         The wall time (ticks).
 * @param [in]    position_error   Current position error (control units).
 */
void pbio_speed_integrator_pause(pbio_speed_integrator_t *itg, uint32_t time_now, int32_t position_error) {

    // Pause only if running
    if (!itg->running) {
        return;
    }

    // The integrator is not running anymore
    itg->running = false;

    // Increment the paused integrator state with the integrated amount between the last resume and the newly enforced pause
    itg->speed_err_integral_paused += position_error - itg->position_error_resumed;

    // Store time at which we started pausing, used only for stall flag hysteresis.
    itg->time_pause_begin = time_now;
}

/**
 * Resumes the speed integrator from the current position error.
 *
 * @param [in]    itg              Speed integrator instance.
 * @param [in]    position_error   Current position error (control units).
 */
void pbio_speed_integrator_resume(pbio_speed_integrator_t *itg, int32_t position_error) {

    // Resume only if paused
    if (itg->running) {
        return;
    }

    // The integrator is running again
    itg->running = true;

    // Set starting point from which we resume, if needed
    // Begin integrating again from the current point
    itg->position_error_resumed = position_error;
}

/**
 * Resets the speed integrator state.
 *
 * @param [in]    itg       Speed integrator instance.
 * @param [in]    settings  Control settings instance from which to read stall settings.
 */
void pbio_speed_integrator_reset(pbio_speed_integrator_t *itg, pbio_control_settings_t *settings) {

    // Save reference to settings.
    itg->settings = settings;

    // Reset built up integral to 0.
    itg->speed_err_integral_paused = 0;

    // Set state to paused. It will resume immediately on start.
    itg->running = false;

    // Resume integration
    pbio_speed_integrator_resume(itg, 0);
}

/**
 * Gets the speed error integral accumulated thus far.
 *
 * @param [in]    itg              Speed integrator instance.
 * @param [in]    position_error   Current position error (control units).
 * @return                         Speed error integral (position control units).
 */
int32_t pbio_speed_integrator_get_error(const pbio_speed_integrator_t *itg, int32_t position_error) {

    // The speed error integral is at least the value at which we paused it last
    int32_t speed_err_integral = itg->speed_err_integral_paused;

    // If integrator is active, add the exact integral since its last restart
    if (itg->running) {
        speed_err_integral += position_error - itg->position_error_resumed;
    }
    return speed_err_integral;
}

/**
 * Checks if the speed integrator state indicates stalling.
 *
 * @param [in]    itg              Speed integrator instance.
 * @param [in]    time_now         The wall time (ticks).
 * @param [in]    speed_now        Current speed (control units).
 * @param [in]    speed_ref        Reference speed (control units).
 * @return                         True if stalled, false if not.
 */
bool pbio_speed_integrator_stalled(const pbio_speed_integrator_t *itg, uint32_t time_now, int32_t speed_now, int32_t speed_ref) {
    // If were running, we're not stalled
    if (itg->running) {
        return false;
    }

    // Equivalent to checking both directions, flip to positive for simplicity.
    if (speed_ref < 0) {
        speed_ref *= -1;
        speed_now *= -1;
    }

    // If we're still running faster than the stall limit, we're certainly not stalled.
    if (speed_ref != 0 && speed_now > itg->settings->stall_speed_limit) {
        return false;
    }

    // If the integrator is paused for less than the stall time, we're still not stalled for now.
    if (!pbio_control_settings_time_is_later(time_now, itg->time_pause_begin + itg->settings->stall_time)) {
        return false;
    }

    // All checks have failed, so we are stalled
    return true;
}

/**
 * Gets reference time compensated for stall duration of position controller.
 *
 * @param [in]    itg              Position integrator instance.
 * @param [in]    time_now         The wall time (ticks).
 * @return                         Wall time compensated for time spent stalling.
 */
uint32_t pbio_position_integrator_get_ref_time(const pbio_position_integrator_t *itg, uint32_t time_now) {
    // The wall time at which we are is either the current time, or whenever we stopped last.
    uint32_t real_time = itg->trajectory_running ? time_now : itg->time_pause_begin;

    // But we want to evaluate the reference compensating for the time we spent waiting.
    return real_time - itg->time_paused_total;
}

/**
 * Pauses the position integrator at the current time.
 *
 * @param [in]    itg              Speed integrator instance.
 * @param [in]    time_now         The wall time (ticks).
 */
void pbio_position_integrator_pause(pbio_position_integrator_t *itg, uint32_t time_now) {

    // Return if already paused.
    if (!itg->trajectory_running) {
        return;
    }

    // Disable the integrator.
    itg->trajectory_running = false;
    itg->time_pause_begin = time_now;
}

/**
 * Tests if the position integrator is paused.
 *
 * @param [in]    itg              Speed integrator instance.
 * @return                         True if integration is paused, false if not.
 */
bool pbio_position_integrator_is_paused(const pbio_position_integrator_t *itg) {
    return !itg->trajectory_running;
}

/**
 * Resumes the position integrator at the current time.
 *
 * @param [in]    itg              Speed integrator instance.
 * @param [in]    time_now         The wall time (ticks).
 */
void pbio_position_integrator_resume(pbio_position_integrator_t *itg, uint32_t time_now) {

    // Return if already trajectory_running
    if (itg->trajectory_running) {
        return;
    }

    // Then we must restart the time
    itg->trajectory_running = true;

    // Increment total wait time by time elapsed since we started pausing
    itg->time_paused_total += time_now - itg->time_pause_begin;
}

/**
 * Resets the position integrator state.
 *
 * @param [in]    itg       Speed integrator instance.
 * @param [in]    settings  Control settings instance from which to read stall settings.
 * @param [in]    time_now  The wall time (ticks).
 */
void pbio_position_integrator_reset(pbio_position_integrator_t *itg, pbio_control_settings_t *settings, uint32_t time_now) {

    // Save reference to settings.
    itg->settings = settings;

    // Reset integrator state variables
    itg->count_err_integral = 0;
    itg->time_paused_total = 0;
    itg->time_pause_begin = time_now;
    itg->trajectory_running = false;

    // Resume integration
    pbio_position_integrator_resume(itg, time_now);

}

/**
 * Updates the position integrator state with the latest error.
 *
 * @param [in]    itg              Speed integrator instance.
 * @param [in]    position_error   Current position error (position control units).
 * @param [in]    target_error     Remaining error to the endpoint (position control units).
 * @return                         Integrator state value (position control units).
 */
int32_t pbio_position_integrator_update(pbio_position_integrator_t *itg, int32_t position_error, int32_t target_error) {

    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);

    // Integrate and update position error
    if (itg->trajectory_running || decrease) {

        // If not deceasing, so growing, limit error growth by maximum integral rate
        if (!decrease) {
            error_now = error_now > itg->settings->integral_change_max ? itg->settings->integral_change_max : error_now;
            error_now = error_now < -itg->settings->integral_change_max ? -itg->settings->integral_change_max : error_now;

            // It might be decreasing now after all (due to integral sign change), so re-evaluate
            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);
        }

        // Specify in which region integral control should be active. This is
        // at least the error that would still lead to maximum  proportional
        // control, with a factor of 2 so we begin integrating a bit sooner.
        int32_t integral_range_upper = pbio_control_settings_div_by_gain(itg->settings->actuation_max, itg->settings->pid_kp) * 2;

        // Add change if we are near (but not too near) target, or always if it decreases the integral magnitude.
        if ((pbio_int_math_abs(target_error) >= itg->settings->integral_deadzone &&
             pbio_int_math_abs(target_error) <= integral_range_upper) || decrease) {
            itg->count_err_integral += pbio_control_settings_mul_by_loop_time(error_now);
        }

        // Limit integral to value that leads to maximum actuation, i.e. max actuation / ki.
        itg->count_err_integral = pbio_int_math_clamp(itg->count_err_integral,
            pbio_control_settings_div_by_gain(itg->settings->actuation_max, itg->settings->pid_ki));
    }

    // Return current value.
    return itg->count_err_integral;
}

/**
 * Checks if the position integrator state indicates stalling.
 *
 * @param [in]    itg              Speed integrator instance.
 * @param [in]    time_now         The wall time (ticks).
 * @param [in]    speed_now        Current speed (control units).
 * @param [in]    speed_ref        Reference speed (control units).
 * @return                         True if stalled, false if not.
 */
bool pbio_position_integrator_stalled(const pbio_position_integrator_t *itg, uint32_t time_now, int32_t speed_now, int32_t speed_ref) {

    // Get integral value that would lead to maximum actuation.
    int32_t integral_max = pbio_control_settings_div_by_gain(itg->settings->actuation_max, itg->settings->pid_ki);

    // Whether the integrator is saturated.
    bool saturated = integral_max != 0 && pbio_int_math_abs(itg->count_err_integral) >= integral_max;

    // If we're running and the integrator is not saturated, we're not stalled.
    if (itg->trajectory_running && !saturated) {
        return false;
    }

    // Equivalent to checking both directions, flip to positive for simplicity.
    if (speed_ref < 0) {
        speed_ref *= -1;
        speed_now *= -1;
    }

    // If we're still running faster than the stall limit, we're certainly not stalled.
    if (speed_ref != 0 && speed_now > itg->settings->stall_speed_limit) {
        return false;
    }

    // If the integrator is paused for less than the stall time, we're still not stalled for now.
    if (!pbio_control_settings_time_is_later(time_now, itg->time_pause_begin + itg->settings->stall_time)) {
        return false;
    }

    // All checks have failed, so we are stalled
    return true;
};

1	// SPDX-License-Identifier: MIT
2	// Copyright (c) 2018-2023 The Pybricks Authors
3
4	// SPDX-License-Identifier: BSD-3-Clause
5	// Copyright (c) 2020-2023 LEGO System A/S
6
7	#include <stdbool.h>
8	#include <stdint.h>
9	#include <stdlib.h>
10
11	#include <pbio/control_settings.h>
12	#include <pbio/integrator.h>
13	#include <pbio/int_math.h>
14
15	/**
16	* Pauses the speed integrator at the current position error.
17	*
18	* @param [in] itg Speed integrator instance.
19	* @param [in] time_now The wall time (ticks).
20	* @param [in] position_error Current position error (control units).
21	*/
22	void pbio_speed_integrator_pause(pbio_speed_integrator_t *itg, uint32_t time_now, int32_t position_error) {	582✔
23
24	// Pause only if running
25	if (!itg->running) {	582✔
26	return;	482✔
27	}
28
29	// The integrator is not running anymore
30	itg->running = false;	6✔
31
32	// Increment the paused integrator state with the integrated amount between the last resume and the newly enforced pause
33	itg->speed_err_integral_paused += position_error - itg->position_error_resumed;	6✔
34
35	// Store time at which we started pausing, used only for stall flag hysteresis.
36	itg->time_pause_begin = time_now;	6✔
37	}
38
39	/**
40	* Resumes the speed integrator from the current position error.
41	*
42	* @param [in] itg Speed integrator instance.
43	* @param [in] position_error Current position error (control units).
44	*/
45	void pbio_speed_integrator_resume(pbio_speed_integrator_t *itg, int32_t position_error) {	1,789✔
46
47	// Resume only if paused
48	if (itg->running) {	1,789✔
49	return;	1,315✔
50	}
51
52	// The integrator is running again
53	itg->running = true;	9✔
54
55	// Set starting point from which we resume, if needed
56	// Begin integrating again from the current point
57	itg->position_error_resumed = position_error;	9✔
58	}
59
60	/**
61	* Resets the speed integrator state.
62	*
63	* @param [in] itg Speed integrator instance.
64	* @param [in] settings Control settings instance from which to read stall settings.
65	*/
66	void pbio_speed_integrator_reset(pbio_speed_integrator_t itg, pbio_control_settings_t settings) {	6✔
67
68	// Save reference to settings.
69	itg->settings = settings;	6✔
70
71	// Reset built up integral to 0.
72	itg->speed_err_integral_paused = 0;	6✔
73
74	// Set state to paused. It will resume immediately on start.
75	itg->running = false;	6✔
76
77	// Resume integration
78	pbio_speed_integrator_resume(itg, 0);	6✔
79	}	6✔
80
81	/**
82	* Gets the speed error integral accumulated thus far.
83	*
84	* @param [in] itg Speed integrator instance.
85	* @param [in] position_error Current position error (control units).
86	* @return Speed error integral (position control units).
87	*/
88	int32_t pbio_speed_integrator_get_error(const pbio_speed_integrator_t *itg, int32_t position_error) {	2,369✔
89
90	// The speed error integral is at least the value at which we paused it last
91	int32_t speed_err_integral = itg->speed_err_integral_paused;	2,369✔
92
93	// If integrator is active, add the exact integral since its last restart
94	if (itg->running) {	2,369✔
95	speed_err_integral += position_error - itg->position_error_resumed;	1,790✔
96	}
97	return speed_err_integral;	2,369✔
98	}
99
100	/**
101	* Checks if the speed integrator state indicates stalling.
102	*
103	* @param [in] itg Speed integrator instance.
104	* @param [in] time_now The wall time (ticks).
105	* @param [in] speed_now Current speed (control units).
106	* @param [in] speed_ref Reference speed (control units).
107	* @return True if stalled, false if not.
108	*/
109	bool pbio_speed_integrator_stalled(const pbio_speed_integrator_t *itg, uint32_t time_now, int32_t speed_now, int32_t speed_ref) {	2,365✔
110	// If were running, we're not stalled
111	if (itg->running) {	2,365✔
112	return false;	1,316✔
113	}
114
115	// Equivalent to checking both directions, flip to positive for simplicity.
116	if (speed_ref < 0) {	582✔
117	speed_ref *= -1;	352✔
118	speed_now *= -1;	352✔
119	}
120
121	// If we're still running faster than the stall limit, we're certainly not stalled.
122	if (speed_ref != 0 && speed_now > itg->settings->stall_speed_limit) {	582✔
123	return false;	30✔
124	}
125
126	// If the integrator is paused for less than the stall time, we're still not stalled for now.
127	if (!pbio_control_settings_time_is_later(time_now, itg->time_pause_begin + itg->settings->stall_time)) {	533✔
128	return false;	127✔
129	}
130
131	// All checks have failed, so we are stalled
132	return true;	404✔
133	}
134
135	/**
136	* Gets reference time compensated for stall duration of position controller.
137	*
138	* @param [in] itg Position integrator instance.
139	* @param [in] time_now The wall time (ticks).
140	* @return Wall time compensated for time spent stalling.
141	*/
142	uint32_t pbio_position_integrator_get_ref_time(const pbio_position_integrator_t *itg, uint32_t time_now) {	14,116✔
143	// The wall time at which we are is either the current time, or whenever we stopped last.
144	uint32_t real_time = itg->trajectory_running ? time_now : itg->time_pause_begin;	14,116✔
145
146	// But we want to evaluate the reference compensating for the time we spent waiting.
147	return real_time - itg->time_paused_total;	14,116✔
148	}
149
150	/**
151	* Pauses the position integrator at the current time.
152	*
153	* @param [in] itg Speed integrator instance.
154	* @param [in] time_now The wall time (ticks).
155	*/
156	void pbio_position_integrator_pause(pbio_position_integrator_t *itg, uint32_t time_now) {	×
157
158	// Return if already paused.
159	if (!itg->trajectory_running) {	×
160	return;
161	}
162
163	// Disable the integrator.
164	itg->trajectory_running = false;	×
165	itg->time_pause_begin = time_now;	×
166	}
167
168	/**
169	* Tests if the position integrator is paused.
170	*
171	* @param [in] itg Speed integrator instance.
172	* @return True if integration is paused, false if not.
173	*/
174	bool pbio_position_integrator_is_paused(const pbio_position_integrator_t *itg) {	×
175	return !itg->trajectory_running;	×
176	}
177
178	/**
179	* Resumes the position integrator at the current time.
180	*
181	* @param [in] itg Speed integrator instance.
182	* @param [in] time_now The wall time (ticks).
183	*/
184	void pbio_position_integrator_resume(pbio_position_integrator_t *itg, uint32_t time_now) {	14,092✔
185
186	// Return if already trajectory_running
187	if (itg->trajectory_running) {	14,092✔
188	return;	6,686✔
189	}
190
191	// Then we must restart the time
192	itg->trajectory_running = true;	16✔
193
194	// Increment total wait time by time elapsed since we started pausing
195	itg->time_paused_total += time_now - itg->time_pause_begin;	16✔
196	}
197
198	/**
199	* Resets the position integrator state.
200	*
201	* @param [in] itg Speed integrator instance.
202	* @param [in] settings Control settings instance from which to read stall settings.
203	* @param [in] time_now The wall time (ticks).
204	*/
205	void pbio_position_integrator_reset(pbio_position_integrator_t itg, pbio_control_settings_t settings, uint32_t time_now) {	16✔
206
207	// Save reference to settings.
208	itg->settings = settings;	16✔
209
210	// Reset integrator state variables
211	itg->count_err_integral = 0;	16✔
212	itg->time_paused_total = 0;	16✔
213	itg->time_pause_begin = time_now;	16✔
214	itg->trajectory_running = false;	16✔
215
216	// Resume integration
217	pbio_position_integrator_resume(itg, time_now);	16✔
218
219	}	16✔
220
221	/**
222	* Updates the position integrator state with the latest error.
223	*
224	* @param [in] itg Speed integrator instance.
225	* @param [in] position_error Current position error (position control units).
226	* @param [in] target_error Remaining error to the endpoint (position control units).
227	* @return Integrator state value (position control units).
228	*/
229	int32_t pbio_position_integrator_update(pbio_position_integrator_t *itg, int32_t position_error, int32_t target_error) {	14,076✔
230
231	int32_t error_now = position_error;	14,076✔
232
233	// Check if integrator magnitude would decrease due to this error
234	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);	14,076✔
235
236	// Integrate and update position error
237	if (itg->trajectory_running \|\| decrease) {	14,076✔
238
239	// If not deceasing, so growing, limit error growth by maximum integral rate
240	if (!decrease) {	14,076✔
241	error_now = error_now > itg->settings->integral_change_max ? itg->settings->integral_change_max : error_now;	13,416✔
242	error_now = error_now < -itg->settings->integral_change_max ? -itg->settings->integral_change_max : error_now;	13,416✔
243
244	// It might be decreasing now after all (due to integral sign change), so re-evaluate
245	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);	13,416✔
246	}
247
248	// Specify in which region integral control should be active. This is
249	// at least the error that would still lead to maximum proportional
250	// control, with a factor of 2 so we begin integrating a bit sooner.
251	int32_t integral_range_upper = pbio_control_settings_div_by_gain(itg->settings->actuation_max, itg->settings->pid_kp) * 2;	14,076✔
252
253	// Add change if we are near (but not too near) target, or always if it decreases the integral magnitude.
254	if ((pbio_int_math_abs(target_error) >= itg->settings->integral_deadzone &&	20,017✔
255	pbio_int_math_abs(target_error) <= integral_range_upper) \|\| decrease) {	19,180✔
256	itg->count_err_integral += pbio_control_settings_mul_by_loop_time(error_now);	1,495✔
257	}
258
259	// Limit integral to value that leads to maximum actuation, i.e. max actuation / ki.
260	itg->count_err_integral = pbio_int_math_clamp(itg->count_err_integral,	14,076✔
261	pbio_control_settings_div_by_gain(itg->settings->actuation_max, itg->settings->pid_ki));	14,076✔
262	}
263
264	// Return current value.
265	return itg->count_err_integral;	14,076✔
266	}
267
268	/**
269	* Checks if the position integrator state indicates stalling.
270	*
271	* @param [in] itg Speed integrator instance.
272	* @param [in] time_now The wall time (ticks).
273	* @param [in] speed_now Current speed (control units).
274	* @param [in] speed_ref Reference speed (control units).
275	* @return True if stalled, false if not.
276	*/
277	bool pbio_position_integrator_stalled(const pbio_position_integrator_t *itg, uint32_t time_now, int32_t speed_now, int32_t speed_ref) {	14,076✔
278
279	// Get integral value that would lead to maximum actuation.
280	int32_t integral_max = pbio_control_settings_div_by_gain(itg->settings->actuation_max, itg->settings->pid_ki);	14,076✔
281
282	// Whether the integrator is saturated.
283	bool saturated = integral_max != 0 && pbio_int_math_abs(itg->count_err_integral) >= integral_max;	14,076✔
284
285	// If we're running and the integrator is not saturated, we're not stalled.
286	if (itg->trajectory_running && !saturated) {	14,076✔
287	return false;	6,686✔
288	}
289
290	// Equivalent to checking both directions, flip to positive for simplicity.
291	if (speed_ref < 0) {	×
292	speed_ref *= -1;	×
293	speed_now *= -1;	×
294	}
295
296	// If we're still running faster than the stall limit, we're certainly not stalled.
297	if (speed_ref != 0 && speed_now > itg->settings->stall_speed_limit) {	×
298	return false;
299	}
300
301	// If the integrator is paused for less than the stall time, we're still not stalled for now.
302	if (!pbio_control_settings_time_is_later(time_now, itg->time_pause_begin + itg->settings->stall_time)) {	×
303	return false;	×
304	}
305
306	// All checks have failed, so we are stalled
307	return true;
308	};

pybricks / pybricks-micropython / 6675885095

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous